From 5456a3bb9111d6bfa4644759053417dba46080ba Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Tue, 30 Jul 2024 13:40:28 -0700 Subject: [PATCH 1/6] Add 3d odometry and pose estimation --- .../DifferentialDrivePoseEstimator3d.java | 274 ++++++++ .../MecanumDrivePoseEstimator3d.java | 143 ++++ .../first/math/estimator/PoseEstimator3d.java | 610 ++++++++++++++++++ .../estimator/SwerveDrivePoseEstimator3d.java | 172 +++++ .../DifferentialDriveOdometry3d.java | 264 ++++++++ .../kinematics/MecanumDriveOdometry3d.java | 83 +++ .../wpi/first/math/kinematics/Odometry3d.java | 228 +++++++ .../kinematics/SwerveDriveOdometry3d.java | 134 ++++ .../DifferentialDrivePoseEstimator3d.cpp | 44 ++ .../estimator/MecanumDrivePoseEstimator3d.cpp | 46 ++ .../estimator/SwerveDrivePoseEstimator3d.cpp | 12 + .../DifferentialDriveOdometry3d.cpp | 29 + .../cpp/kinematics/MecanumDriveOdometry3d.cpp | 29 + .../cpp/kinematics/SwerveDriveOdometry3d.cpp | 12 + .../DifferentialDrivePoseEstimator3d.h | 242 +++++++ .../estimator/MecanumDrivePoseEstimator3d.h | 128 ++++ .../include/frc/estimator/PoseEstimator3d.h | 422 ++++++++++++ .../include/frc/estimator/PoseEstimator3d.inc | 339 ++++++++++ .../estimator/SwerveDrivePoseEstimator3d.h | 152 +++++ .../kinematics/DifferentialDriveOdometry3d.h | 152 +++++ .../frc/kinematics/MecanumDriveOdometry3d.h | 61 ++ .../include/frc/kinematics/Odometry3d.h | 205 ++++++ .../include/frc/kinematics/Odometry3d.inc | 62 ++ .../frc/kinematics/SwerveDriveOdometry3d.h | 72 +++ .../frc/kinematics/SwerveDriveOdometry3d.inc | 36 ++ 25 files changed, 3951 insertions(+) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java create mode 100644 wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp create mode 100644 wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp create mode 100644 wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator3d.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp create mode 100644 wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h create mode 100644 wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h create mode 100644 wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h create mode 100644 wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.inc create mode 100644 wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/Odometry3d.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/Odometry3d.inc create mode 100644 wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.inc diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java new file mode 100644 index 00000000000..8434e3eedff --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java @@ -0,0 +1,274 @@ +// 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. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry3d; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N4; + +/** + * This class wraps {@link DifferentialDriveOdometry3d Differential Drive Odometry} to fuse + * latency-compensated vision measurements with differential drive encoder measurements. It is + * intended to be a drop-in replacement for {@link DifferentialDriveOdometry3d}; in fact, if you + * never call {@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} and only call {@link + * DifferentialDrivePoseEstimator3d#update} then this will behave exactly the same as + * DifferentialDriveOdometry3d. + * + *

{@link DifferentialDrivePoseEstimator3d#update} should be called every robot loop. + * + *

{@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as + * you want; if you never call it then this class will behave exactly like regular encoder odometry. + */ +public class DifferentialDrivePoseEstimator3d + extends PoseEstimator3d { + /** + * Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model + * and vision measurements. + * + *

The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for + * y, and 0.01 radians for heading. The default standard deviations of the vision measurements are + * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param initialPoseMeters The starting pose estimate. + */ + public DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics kinematics, + Rotation2d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose2d initialPoseMeters) { + this( + kinematics, + gyroAngle, + leftDistanceMeters, + rightDistanceMeters, + initialPoseMeters, + VecBuilder.fill(0.02, 0.02, 0.01), + VecBuilder.fill(0.1, 0.1, 0.1)); + } + + /** + * Constructs a DifferentialDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The gyro angle of the robot. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param initialPoseMeters The estimated initial pose. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and heading in radians). Increase these numbers to trust your state estimate + * less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics kinematics, + Rotation2d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose2d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new DifferentialDriveOdometry3d( + gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); + } + + /** + * Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model + * and vision measurements. + * + *

The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for + * y, 0.02 meters for z, and 0.01 radians for angle. The default standard deviations of the vision + * measurements are 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for + * angle. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param initialPoseMeters The starting pose estimate. + */ + public DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics kinematics, + Rotation3d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose3d initialPoseMeters) { + this( + kinematics, + gyroAngle, + leftDistanceMeters, + rightDistanceMeters, + initialPoseMeters, + VecBuilder.fill(0.02, 0.02, 0.02, 0.01), + VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); + } + + /** + * Constructs a DifferentialDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The gyro angle of the robot. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param initialPoseMeters The estimated initial pose. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and heading in radians). Increase these numbers to trust your state estimate + * less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics kinematics, + Rotation3d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose3d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new DifferentialDriveOdometry3d( + gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs, + N3.instance); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param leftPositionMeters The distance traveled by the left encoder. + * @param rightPositionMeters The distance traveled by the right encoder. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation2d gyroAngle, + double leftPositionMeters, + double rightPositionMeters, + Pose2d poseMeters) { + resetPosition( + gyroAngle, + new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters), + poseMeters); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param leftPositionMeters The distance traveled by the left encoder. + * @param rightPositionMeters The distance traveled by the right encoder. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation3d gyroAngle, + double leftPositionMeters, + double rightPositionMeters, + Pose3d poseMeters) { + resetPosition( + gyroAngle, + new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters), + poseMeters); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param gyroAngle The current gyro angle. + * @param distanceLeftMeters The total distance travelled by the left wheel in meters. + * @param distanceRightMeters The total distance travelled by the right wheel in meters. + * @return The estimated pose of the robot in meters. + */ + public Pose2d update( + Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) { + return update( + gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param gyroAngle The current gyro angle. + * @param distanceLeftMeters The total distance travelled by the left wheel in meters. + * @param distanceRightMeters The total distance travelled by the right wheel in meters. + * @return The estimated pose of the robot in meters. + */ + public Pose3d update( + Rotation3d gyroAngle, double distanceLeftMeters, double distanceRightMeters) { + return update( + gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyro angle. + * @param distanceLeftMeters The total distance travelled by the left wheel in meters. + * @param distanceRightMeters The total distance travelled by the right wheel in meters. + * @return The estimated pose of the robot in meters. + */ + public Pose2d updateWithTime( + double currentTimeSeconds, + Rotation2d gyroAngle, + double distanceLeftMeters, + double distanceRightMeters) { + return updateWithTime( + currentTimeSeconds, + gyroAngle, + new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyro angle. + * @param distanceLeftMeters The total distance travelled by the left wheel in meters. + * @param distanceRightMeters The total distance travelled by the right wheel in meters. + * @return The estimated pose of the robot in meters. + */ + public Pose3d updateWithTime( + double currentTimeSeconds, + Rotation3d gyroAngle, + double distanceLeftMeters, + double distanceRightMeters) { + return updateWithTime( + currentTimeSeconds, + gyroAngle, + new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java new file mode 100644 index 00000000000..b852f31004c --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java @@ -0,0 +1,143 @@ +// 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. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import edu.wpi.first.math.kinematics.MecanumDriveOdometry3d; +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N4; + +/** + * This class wraps {@link MecanumDriveOdometry3d Mecanum Drive Odometry} to fuse + * latency-compensated vision measurements with mecanum drive encoder distance measurements. It will + * correct for noisy measurements and encoder drift. It is intended to be a drop-in replacement for + * {@link edu.wpi.first.math.kinematics.MecanumDriveOdometry3d}. + * + *

{@link MecanumDrivePoseEstimator3d#update} should be called every robot loop. + * + *

{@link MecanumDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as you + * want; if you never call it, then this class will behave mostly like regular encoder odometry. + */ +public class MecanumDrivePoseEstimator3d extends PoseEstimator3d { + /** + * Constructs a MecanumDrivePoseEstimator3d with default standard deviations for the model and + * vision measurements. + * + *

The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, + * and 0.1 radians for heading. The default standard deviations of the vision measurements are + * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances driven by each wheel. + * @param initialPoseMeters The starting pose estimate. + */ + public MecanumDrivePoseEstimator3d( + MecanumDriveKinematics kinematics, + Rotation2d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose2d initialPoseMeters) { + this( + kinematics, + gyroAngle, + wheelPositions, + initialPoseMeters, + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.45, 0.45, 0.45)); + } + + /** + * Constructs a MecanumDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distance measured by each wheel. + * @param initialPoseMeters The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and heading in radians). Increase these numbers to trust your state estimate + * less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public MecanumDrivePoseEstimator3d( + MecanumDriveKinematics kinematics, + Rotation2d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose2d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); + } + + /** + * Constructs a MecanumDrivePoseEstimator3d with default standard deviations for the model and + * vision measurements. + * + *

The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, + * 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision + * measurements are 0.45 meters for x, 0.45 meters for y, 0.45 meters for z, and 0.45 radians for + * angle. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances driven by each wheel. + * @param initialPoseMeters The starting pose estimate. + */ + public MecanumDrivePoseEstimator3d( + MecanumDriveKinematics kinematics, + Rotation3d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose3d initialPoseMeters) { + this( + kinematics, + gyroAngle, + wheelPositions, + initialPoseMeters, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.45, 0.45, 0.45, 0.45)); + } + + /** + * Constructs a MecanumDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distance measured by each wheel. + * @param initialPoseMeters The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and heading in radians). Increase these numbers to trust your state estimate + * less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public MecanumDrivePoseEstimator3d( + MecanumDriveKinematics kinematics, + Rotation3d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose3d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs, + N3.instance); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java new file mode 100644 index 00000000000..25ca1b5b75b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -0,0 +1,610 @@ +// 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. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.kinematics.Kinematics; +import edu.wpi.first.math.kinematics.Odometry3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N4; +import edu.wpi.first.math.numbers.N6; +import java.util.NavigableMap; +import java.util.Optional; +import java.util.TreeMap; + +/** + * This class wraps {@link Odometry3d} to fuse latency-compensated vision measurements with encoder + * measurements. Robot code should not use this directly- Instead, use the particular type for your + * drivetrain (e.g., {@link DifferentialDrivePoseEstimator3d}). It is intended to be a drop-in + * replacement for {@link Odometry3d}; in fact, if you never call {@link + * PoseEstimator3d#addVisionMeasurement} and only call {@link PoseEstimator3d#update} then this will + * behave exactly the same as Odometry3d. + * + *

{@link PoseEstimator3d#update} should be called every robot loop. + * + *

{@link PoseEstimator3d#addVisionMeasurement} can be called as infrequently as you want; if you + * never call it then this class will behave exactly like regular encoder odometry. + * + * @param Wheel positions type. + */ +public class PoseEstimator3d { + private final Odometry3d m_odometry; + private final Matrix m_q = new Matrix<>(Nat.N4(), Nat.N1()); + private final Matrix m_visionK = new Matrix<>(Nat.N6(), Nat.N6()); + + private static final double kBufferDuration = 1.5; + // Maps timestamps to odometry-only pose estimates + private final TimeInterpolatableBuffer m_odometryPoseBuffer = + TimeInterpolatableBuffer.createBuffer(kBufferDuration); + // Maps timestamps to vision updates + // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have + // been no vision measurements after the last reset + private final NavigableMap m_visionUpdates = new TreeMap<>(); + + private Pose3d m_poseEstimate; + + /** + * Constructs a PoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param odometry A correctly-configured odometry object for your drivetrain. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and angle in radians). Increase these numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public PoseEstimator3d( + Kinematics kinematics, + Odometry3d odometry, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + this( + kinematics, + odometry, + VecBuilder.fill(stateStdDevs.get(0, 0), stateStdDevs.get(1, 0), 0, stateStdDevs.get(2, 0)), + VecBuilder.fill( + visionMeasurementStdDevs.get(0, 0), + visionMeasurementStdDevs.get(1, 0), + 0, + visionMeasurementStdDevs.get(2, 0)), + N3.instance); + } + + /** + * Constructs a PoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param odometry A correctly-configured odometry object for your drivetrain. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, z position in meters, and angle in radians). Increase these numbers to trust + * your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, z position in meters, and angle in radians). Increase + * these numbers to trust the vision pose measurement less. + * @param dimension Marker parameter to differentiate the 2D and 3D cases. Necessary because type + * erasure would otherwise make this look identical to the 2D version. + */ + @SuppressWarnings("PMD.UnusedFormalParameter") + public PoseEstimator3d( + Kinematics kinematics, + Odometry3d odometry, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs, + N3 dimension) { + m_odometry = odometry; + + m_poseEstimate = m_odometry.getPose3dMeters(); + + for (int i = 0; i < 4; ++i) { + m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); + } + setVisionMeasurementStdDevs3d(visionMeasurementStdDevs); + } + + /** + * Sets the pose estimator's trust of global measurements. This might be used to change trust in + * vision measurements after the autonomous period, or to change trust as distance to a vision + * target increases. + * + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these + * numbers to trust global measurements from vision less. This matrix is in the form [x, y, + * theta]ᵀ, with units in meters and radians. + */ + public final void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { + setVisionMeasurementStdDevs3d( + VecBuilder.fill( + visionMeasurementStdDevs.get(0, 0), + visionMeasurementStdDevs.get(1, 0), + 0, + visionMeasurementStdDevs.get(2, 0))); + } + + /** + * Sets the pose estimator's trust of global measurements. This might be used to change trust in + * vision measurements after the autonomous period, or to change trust as distance to a vision + * target increases. + * + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these + * numbers to trust global measurements from vision less. This matrix is in the form [x, y, z, + * theta]ᵀ, with units in meters and radians. + */ + public final void setVisionMeasurementStdDevs3d(Matrix visionMeasurementStdDevs) { + var r = new double[4]; + for (int i = 0; i < 4; ++i) { + r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0); + } + + // Solve for closed form Kalman gain for continuous Kalman filter with A = 0 + // and C = I. See wpimath/algorithms.md. + for (int row = 0; row < 4; ++row) { + if (m_q.get(row, 0) == 0.0) { + m_visionK.set(row, row, 0.0); + } else { + m_visionK.set( + row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row]))); + } + } + // Fill in the gains for the other components of the rotation vector + double angle_gain = m_visionK.get(3, 3); + m_visionK.set(4, 4, angle_gain); + m_visionK.set(5, 5, angle_gain); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { + resetPosition( + new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions, new Pose3d(poseMeters)); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) { + // Reset state estimate and error covariance + m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters); + m_odometryPoseBuffer.clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.getPose3dMeters(); + } + + /** + * Resets the robot's pose. + * + * @param pose The pose to reset to. + */ + public void resetPose(Pose2d pose) { + resetPose(new Pose3d(pose)); + } + + /** + * Resets the robot's pose. + * + * @param pose The pose to reset to. + */ + public void resetPose(Pose3d pose) { + m_odometry.resetPose(pose); + m_odometryPoseBuffer.clear(); + } + + /** + * Resets the robot's translation. + * + * @param translation The pose to translation to. + */ + public void resetTranslation(Translation2d translation) { + resetTranslation(new Translation3d(translation.getX(), translation.getY(), 0)); + } + + /** + * Resets the robot's translation. + * + * @param translation The pose to translation to. + */ + public void resetTranslation(Translation3d translation) { + m_odometry.resetTranslation(translation); + m_odometryPoseBuffer.clear(); + } + + /** + * Resets the robot's rotation. + * + * @param rotation The rotation to reset to. + */ + public void resetRotation(Rotation2d rotation) { + resetRotation(new Rotation3d(0, 0, rotation.getRadians())); + } + + /** + * Resets the robot's rotation. + * + * @param rotation The rotation to reset to. + */ + public void resetRotation(Rotation3d rotation) { + m_odometry.resetRotation(rotation); + m_odometryPoseBuffer.clear(); + } + + /** + * Gets the estimated robot pose. + * + * @return The estimated robot pose in meters. + */ + public Pose2d getEstimatedPosition() { + return m_poseEstimate.toPose2d(); + } + + /** + * Gets the estimated robot pose. + * + * @return The estimated robot pose in meters. + */ + public Pose3d getEstimatedPosition3d() { + return m_poseEstimate; + } + + /** + * Return the pose at a given timestamp, if the buffer is not empty. + * + * @param timestampSeconds The pose's timestamp in seconds. + * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). + */ + public Optional sampleAt(double timestampSeconds) { + return sampleAt3d(timestampSeconds).map(pose3d -> pose3d.toPose2d()); + } + + /** + * Return the pose at a given timestamp, if the buffer is not empty. + * + * @param timestampSeconds The pose's timestamp in seconds. + * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). + */ + public Optional sampleAt3d(double timestampSeconds) { + // Step 0: If there are no odometry updates to sample, skip. + if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) { + return Optional.empty(); + } + + // Step 1: Make sure timestamp matches the sample from the odometry pose buffer. (When sampling, + // the buffer will always use a timestamp between the first and last timestamps) + double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey(); + double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey(); + timestampSeconds = + MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp); + + // Step 2: If there are no applicable vision updates, use the odometry-only information. + if (m_visionUpdates.isEmpty() || timestampSeconds < m_visionUpdates.firstKey()) { + return m_odometryPoseBuffer.getSample(timestampSeconds); + } + + // Step 3: Get the latest vision update from before or at the timestamp to sample at. + double floorTimestamp = m_visionUpdates.floorKey(timestampSeconds); + var visionUpdate = m_visionUpdates.get(floorTimestamp); + + // Step 4: Get the pose measured by odometry at the time of the sample. + var odometryEstimate = m_odometryPoseBuffer.getSample(timestampSeconds); + + // Step 5: Apply the vision compensation to the odometry pose. + return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose)); + } + + /** Removes stale vision updates that won't affect sampling. */ + private void cleanUpVisionUpdates() { + // Step 0: If there are no odometry samples, skip. + if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) { + return; + } + + // Step 1: Find the oldest timestamp that needs a vision update. + double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey(); + + // Step 2: If there are no vision updates before that timestamp, skip. + if (m_visionUpdates.isEmpty() || oldestOdometryTimestamp < m_visionUpdates.firstKey()) { + return; + } + + // Step 3: Find the newest vision update timestamp before or at the oldest timestamp. + double newestNeededVisionUpdateTimestamp = m_visionUpdates.floorKey(oldestOdometryTimestamp); + + // Step 4: Remove all entries strictly before the newest timestamp we need. + m_visionUpdates.headMap(newestNeededVisionUpdateTimestamp, false).clear(); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + *

This method can be called as infrequently as you want, as long as you are calling {@link + * PoseEstimator3d#update} every loop. + * + *

To promote stability of the pose estimate and make it robust to bad vision data, we + * recommend only adding vision measurements that are already within one meter or so of the + * current pose estimate. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you + * don't use your own time source by calling {@link + * PoseEstimator3d#updateWithTime(double,Rotation2d,Object)} then you must use a timestamp + * with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as + * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use + * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the + * epochs. + */ + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + addVisionMeasurement(new Pose3d(visionRobotPoseMeters), timestampSeconds); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + *

This method can be called as infrequently as you want, as long as you are calling {@link + * PoseEstimator3d#update} every loop. + * + *

To promote stability of the pose estimate and make it robust to bad vision data, we + * recommend only adding vision measurements that are already within one meter or so of the + * current pose estimate. + * + *

Note that the vision measurement standard deviations passed into this method will continue + * to apply to future measurements until a subsequent call to {@link + * PoseEstimator3d#setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you + * don't use your own time source by calling {@link #updateWithTime}, then you must use a + * timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same + * epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you + * should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in + * this case. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + setVisionMeasurementStdDevs(visionMeasurementStdDevs); + addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + *

This method can be called as infrequently as you want, as long as you are calling {@link + * PoseEstimator3d#update} every loop. + * + *

To promote stability of the pose estimate and make it robust to bad vision data, we + * recommend only adding vision measurements that are already within one meter or so of the + * current pose estimate. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you + * don't use your own time source by calling {@link + * PoseEstimator3d#updateWithTime(double,Rotation2d,Object)} then you must use a timestamp + * with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as + * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use + * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the + * epochs. + */ + public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampSeconds) { + // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. + if (m_odometryPoseBuffer.getInternalBuffer().isEmpty() + || m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration + > timestampSeconds) { + return; + } + + // Step 1: Clean up any old entries + cleanUpVisionUpdates(); + + // Step 2: Get the pose measured by odometry at the moment the vision measurement was made. + var odometrySample = m_odometryPoseBuffer.getSample(timestampSeconds); + + if (odometrySample.isEmpty()) { + return; + } + + // Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was + // made. + var visionSample = sampleAt3d(timestampSeconds); + + if (visionSample.isEmpty()) { + return; + } + + // Step 4: Measure the twist between the old pose estimate and the vision pose. + var twist = visionSample.get().log(visionRobotPoseMeters); + + // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // gain matrix representing how much we trust vision measurements compared to our current pose. + var k_times_twist = + m_visionK.times( + VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz)); + + // Step 6: Convert back to Twist3d. + var scaledTwist = + new Twist3d( + k_times_twist.get(0, 0), + k_times_twist.get(1, 0), + k_times_twist.get(2, 0), + k_times_twist.get(3, 0), + k_times_twist.get(4, 0), + k_times_twist.get(5, 0)); + + // Step 7: Calculate and record the vision update. + var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); + m_visionUpdates.put(timestampSeconds, visionUpdate); + + // Step 8: Remove later vision measurements. (Matches previous behavior) + m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear(); + + // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update, + // it's guaranteed to be the latest vision update. + m_poseEstimate = visionUpdate.compensate(m_odometry.getPose3dMeters()); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + *

This method can be called as infrequently as you want, as long as you are calling {@link + * PoseEstimator3d#update} every loop. + * + *

To promote stability of the pose estimate and make it robust to bad vision data, we + * recommend only adding vision measurements that are already within one meter or so of the + * current pose estimate. + * + *

Note that the vision measurement standard deviations passed into this method will continue + * to apply to future measurements until a subsequent call to {@link + * PoseEstimator3d#setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you + * don't use your own time source by calling {@link #updateWithTime}, then you must use a + * timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same + * epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you + * should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in + * this case. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, z position in meters, and angle in radians). Increase + * these numbers to trust the vision pose measurement less. + */ + public void addVisionMeasurement3d( + Pose3d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + setVisionMeasurementStdDevs3d(visionMeasurementStdDevs); + addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param gyroAngle The current gyro angle. + * @param wheelPositions The current encoder readings. + * @return The estimated pose of the robot in meters. + */ + public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { + return update(new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions).toPose2d(); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param gyroAngle The current gyro angle. + * @param wheelPositions The current encoder readings. + * @return The estimated pose of the robot in meters. + */ + public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { + return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The current encoder readings. + * @return The estimated pose of the robot in meters. + */ + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) { + return updateWithTime( + currentTimeSeconds, new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions) + .toPose2d(); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The current encoder readings. + * @return The estimated pose of the robot in meters. + */ + public Pose3d updateWithTime(double currentTimeSeconds, Rotation3d gyroAngle, T wheelPositions) { + var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions); + + m_odometryPoseBuffer.addSample(currentTimeSeconds, odometryEstimate); + + if (m_visionUpdates.isEmpty()) { + m_poseEstimate = odometryEstimate; + } else { + var visionUpdate = m_visionUpdates.get(m_visionUpdates.lastKey()); + m_poseEstimate = visionUpdate.compensate(odometryEstimate); + } + + return getEstimatedPosition3d(); + } + + /** + * Represents a vision update record. The record contains the vision-compensated pose estimate as + * well as the corresponding odometry pose estimate. + */ + private static final class VisionUpdate { + // The vision-compensated pose estimate. + private final Pose3d visionPose; + + // The pose estimated based solely on odometry. + private final Pose3d odometryPose; + + /** + * Constructs a vision update record with the specified parameters. + * + * @param visionPose The vision-compensated pose estimate. + * @param odometryPose The pose estimate based solely on odometry. + */ + private VisionUpdate(Pose3d visionPose, Pose3d odometryPose) { + this.visionPose = visionPose; + this.odometryPose = odometryPose; + } + + /** + * Returns the vision-compensated version of the pose. Specifically, changes the pose from being + * relative to this record's odometry pose to being relative to this record's vision pose. + * + * @param pose The pose to compensate. + * @return The compensated pose. + */ + public Pose3d compensate(Pose3d pose) { + var delta = pose.minus(this.odometryPose); + return this.visionPose.plus(delta); + } + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java new file mode 100644 index 00000000000..bbb0f92cb2f --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java @@ -0,0 +1,172 @@ +// 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. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry3d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N4; + +/** + * This class wraps {@link SwerveDriveOdometry3d Swerve Drive Odometry} to fuse latency-compensated + * vision measurements with swerve drive encoder distance measurements. It is intended to be a + * drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry3d}. + * + *

{@link SwerveDrivePoseEstimator3d#update} should be called every robot loop. + * + *

{@link SwerveDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as you + * want; if you never call it, then this class will behave as regular encoder odometry. + */ +public class SwerveDrivePoseEstimator3d extends PoseEstimator3d { + private final int m_numModules; + + /** + * Constructs a SwerveDrivePoseEstimator3d with default standard deviations for the model and + * vision measurements. + * + *

The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, + * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 + * meters for x, 0.9 meters for y, and 0.9 radians for heading. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance measurements and rotations of the swerve modules. + * @param initialPoseMeters The starting pose estimate. + */ + public SwerveDrivePoseEstimator3d( + SwerveDriveKinematics kinematics, + Rotation2d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose2d initialPoseMeters) { + this( + kinematics, + gyroAngle, + modulePositions, + initialPoseMeters, + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9)); + } + + /** + * Constructs a SwerveDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance and rotation measurements of the swerve modules. + * @param initialPoseMeters The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and heading in radians). Increase these numbers to trust your state estimate + * less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public SwerveDrivePoseEstimator3d( + SwerveDriveKinematics kinematics, + Rotation2d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose2d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); + + m_numModules = modulePositions.length; + } + + /** + * Constructs a SwerveDrivePoseEstimator3d with default standard deviations for the model and + * vision measurements. + * + *

The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, + * 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision + * measurements are 0.9 meters for x, 0.9 meters for y, 0.9 meters for z, and 0.9 radians for + * angle. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance measurements and rotations of the swerve modules. + * @param initialPoseMeters The starting pose estimate. + */ + public SwerveDrivePoseEstimator3d( + SwerveDriveKinematics kinematics, + Rotation3d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose3d initialPoseMeters) { + this( + kinematics, + gyroAngle, + modulePositions, + initialPoseMeters, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9, 0.9)); + } + + /** + * Constructs a SwerveDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance and rotation measurements of the swerve modules. + * @param initialPoseMeters The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and heading in radians). Increase these numbers to trust your state estimate + * less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public SwerveDrivePoseEstimator3d( + SwerveDriveKinematics kinematics, + Rotation3d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose3d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs, + N3.instance); + + m_numModules = modulePositions.length; + } + + @Override + public Pose2d updateWithTime( + double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) { + if (wheelPositions.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + + return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions); + } + + @Override + public Pose3d updateWithTime( + double currentTimeSeconds, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions) { + if (wheelPositions.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + + return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java new file mode 100644 index 00000000000..167338951c6 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java @@ -0,0 +1,264 @@ +// 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. + +package edu.wpi.first.math.kinematics; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; + +/** + * Class for differential drive odometry. Odometry allows you to track the robot's position on the + * field over the course of a match using readings from 2 encoders and a gyroscope. + * + *

Teams can use odometry during the autonomous period for complex tasks like path following. + * Furthermore, odometry can be used for latency compensation when using computer-vision systems. + * + *

It is important that you reset your encoders to zero before using this class. Any subsequent + * pose resets also require the encoders to be reset to zero. + */ +public class DifferentialDriveOdometry3d extends Odometry3d { + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public DifferentialDriveOdometry3d( + Rotation2d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose2d initialPoseMeters) { + super( + new DifferentialDriveKinematics(1), + gyroAngle, + new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters), + initialPoseMeters); + MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1); + } + + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public DifferentialDriveOdometry3d( + Rotation2d gyroAngle, + Measure leftDistance, + Measure rightDistance, + Pose2d initialPoseMeters) { + this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters); + } + + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + */ + public DifferentialDriveOdometry3d( + Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { + this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose2d.kZero); + } + + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + */ + public DifferentialDriveOdometry3d( + Rotation2d gyroAngle, Measure leftDistance, Measure rightDistance) { + this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero); + } + + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public DifferentialDriveOdometry3d( + Rotation3d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose3d initialPoseMeters) { + super( + new DifferentialDriveKinematics(1), + gyroAngle, + new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters), + initialPoseMeters); + MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1); + } + + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public DifferentialDriveOdometry3d( + Rotation3d gyroAngle, + Measure leftDistance, + Measure rightDistance, + Pose3d initialPoseMeters) { + this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters); + } + + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + */ + public DifferentialDriveOdometry3d( + Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { + this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose3d.kZero); + } + + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + */ + public DifferentialDriveOdometry3d( + Rotation3d gyroAngle, Measure leftDistance, Measure rightDistance) { + this(gyroAngle, leftDistance, rightDistance, Pose3d.kZero); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation2d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose2d poseMeters) { + super.resetPosition( + gyroAngle, + new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters), + poseMeters); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation2d gyroAngle, + Measure leftDistance, + Measure rightDistance, + Pose2d poseMeters) { + resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation3d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose3d poseMeters) { + super.resetPosition( + gyroAngle, + new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters), + poseMeters); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation3d gyroAngle, + Measure leftDistance, + Measure rightDistance, + Pose3d poseMeters) { + resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); + } + + /** + * Updates the robot position on the field using distance measurements from encoders. This method + * is more numerically accurate than using velocities to integrate the pose and is also + * advantageous for teams that are using lower CPR encoders. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @return The new pose of the robot. + */ + public Pose2d update( + Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { + return super.update( + gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters)); + } + + /** + * Updates the robot position on the field using distance measurements from encoders. This method + * is more numerically accurate than using velocities to integrate the pose and is also + * advantageous for teams that are using lower CPR encoders. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @return The new pose of the robot. + */ + public Pose3d update( + Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { + return super.update( + gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters)); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java new file mode 100644 index 00000000000..0c305d694cc --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java @@ -0,0 +1,83 @@ +// 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. + +package edu.wpi.first.math.kinematics; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; + +/** + * Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field + * over a course of a match using readings from your mecanum wheel encoders. + * + *

Teams can use odometry during the autonomous period for complex tasks like path following. + * Furthermore, odometry can be used for latency compensation when using computer-vision systems. + */ +public class MecanumDriveOdometry3d extends Odometry3d { + /** + * Constructs a MecanumDriveOdometry3d object. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances driven by each wheel. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, + Rotation2d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose2d initialPoseMeters) { + super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); + MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1); + } + + /** + * Constructs a MecanumDriveOdometry3d object with the default pose at the origin. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances driven by each wheel. + */ + public MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, + Rotation2d gyroAngle, + MecanumDriveWheelPositions wheelPositions) { + this(kinematics, gyroAngle, wheelPositions, Pose2d.kZero); + } + + /** + * Constructs a MecanumDriveOdometry3d object. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances driven by each wheel. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, + Rotation3d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose3d initialPoseMeters) { + super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); + MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1); + } + + /** + * Constructs a MecanumDriveOdometry3d object with the default pose at the origin. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances driven by each wheel. + */ + public MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, + Rotation3d gyroAngle, + MecanumDriveWheelPositions wheelPositions) { + this(kinematics, gyroAngle, wheelPositions, Pose3d.kZero); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java new file mode 100644 index 00000000000..565bd6633b5 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java @@ -0,0 +1,228 @@ +// 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. + +package edu.wpi.first.math.kinematics; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist3d; + +/** + * Class for odometry. Robot code should not use this directly- Instead, use the particular type for + * your drivetrain (e.g., {@link DifferentialDriveOdometry3d}). Odometry allows you to track the + * robot's position on the field over the course of a match using readings from encoders and a + * gyroscope. + * + *

Teams can use odometry during the autonomous period for complex tasks like path following. + * Furthermore, odometry can be used for latency compensation when using computer-vision systems. + * + * @param Wheel positions type. + */ +public class Odometry3d { + private final Kinematics m_kinematics; + private Pose3d m_poseMeters; + + private Rotation3d m_gyroOffset; + private Rotation3d m_previousAngle; + private final T m_previousWheelPositions; + + /** + * Constructs an Odometry3d object. + * + * @param kinematics The kinematics of the drivebase. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public Odometry3d( + Kinematics kinematics, + Rotation2d gyroAngle, + T wheelPositions, + Pose2d initialPoseMeters) { + this( + kinematics, + new Rotation3d(0, 0, gyroAngle.getRadians()), + wheelPositions, + new Pose3d(initialPoseMeters)); + } + + /** + * Constructs an Odometry3d object. + * + * @param kinematics The kinematics of the drivebase. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public Odometry3d( + Kinematics kinematics, + Rotation3d gyroAngle, + T wheelPositions, + Pose3d initialPoseMeters) { + m_kinematics = kinematics; + m_poseMeters = initialPoseMeters; + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + m_previousAngle = m_poseMeters.getRotation(); + m_previousWheelPositions = m_kinematics.copy(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 gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { + resetPosition( + new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions, new Pose3d(poseMeters)); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) { + m_poseMeters = poseMeters; + m_previousAngle = m_poseMeters.getRotation(); + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); + } + + /** + * Resets the pose. + * + * @param poseMeters The pose to reset to. + */ + public void resetPose(Pose2d poseMeters) { + resetPose(new Pose3d(poseMeters)); + } + + /** + * Resets the pose. + * + * @param poseMeters The pose to reset to. + */ + public void resetPose(Pose3d poseMeters) { + m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation())); + m_poseMeters = poseMeters; + m_previousAngle = m_poseMeters.getRotation(); + } + + /** + * Resets the translation of the pose. + * + * @param translation The translation to reset to. + */ + public void resetTranslation(Translation2d translation) { + resetTranslation(new Translation3d(translation.getX(), translation.getY(), 0)); + } + + /** + * Resets the translation of the pose. + * + * @param translation The translation to reset to. + */ + public void resetTranslation(Translation3d translation) { + m_poseMeters = new Pose3d(translation, m_poseMeters.getRotation()); + } + + /** + * Resets the rotation of the pose. + * + * @param rotation The rotation to reset to. + */ + public void resetRotation(Rotation2d rotation) { + resetRotation(new Rotation3d(0, 0, rotation.getRadians())); + } + + /** + * Resets the rotation of the pose. + * + * @param rotation The rotation to reset to. + */ + public void resetRotation(Rotation3d rotation) { + m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation())); + m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation); + m_previousAngle = m_poseMeters.getRotation(); + } + + /** + * Returns the position of the robot on the field. + * + * @return The pose of the robot (x and y are in meters). + */ + public Pose2d getPoseMeters() { + return m_poseMeters.toPose2d(); + } + + /** + * Returns the position of the robot on the field. + * + * @return The pose of the robot (x, y, and z are in meters). + */ + public Pose3d getPose3dMeters() { + return m_poseMeters; + } + + /** + * Updates the robot's position on the field using forward kinematics and integration of the pose + * over time. This method takes in an angle parameter which is used instead of the angular rate + * that is calculated from forward kinematics, in addition to the current distance measurement at + * each wheel. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @return The new pose of the robot. + */ + public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { + return update(new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions).toPose2d(); + } + + /** + * Updates the robot's position on the field using forward kinematics and integration of the pose + * over time. This method takes in an angle parameter which is used instead of the angular rate + * that is calculated from forward kinematics, in addition to the current distance measurement at + * each wheel. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @return The new pose of the robot. + */ + public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { + var angle = gyroAngle.plus(m_gyroOffset); + var angle_difference = angle.minus(m_previousAngle).getQuaternion().toRotationVector(); + + var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); + var twist = + new Twist3d( + twist2d.dx, + twist2d.dy, + 0, + angle_difference.get(0), + angle_difference.get(1), + angle_difference.get(2)); + + var newPose = m_poseMeters.exp(twist); + + m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); + m_previousAngle = angle; + m_poseMeters = new Pose3d(newPose.getTranslation(), angle); + + return m_poseMeters; + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java new file mode 100644 index 00000000000..878b198dd90 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java @@ -0,0 +1,134 @@ +// 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. + +package edu.wpi.first.math.kinematics; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; + +/** + * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field + * over a course of a match using readings from your swerve drive encoders and swerve azimuth + * encoders. + * + *

Teams can use odometry during the autonomous period for complex tasks like path following. + * Furthermore, odometry can be used for latency compensation when using computer-vision systems. + */ +public class SwerveDriveOdometry3d extends Odometry3d { + private final int m_numModules; + + /** + * Constructs a SwerveDriveOdometry3d object. + * + * @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. + */ + public SwerveDriveOdometry3d( + SwerveDriveKinematics kinematics, + Rotation2d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose2d initialPose) { + super(kinematics, gyroAngle, modulePositions, initialPose); + + m_numModules = modulePositions.length; + + MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1); + } + + /** + * Constructs a SwerveDriveOdometry3d object with the default pose at the origin. + * + * @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. + */ + public SwerveDriveOdometry3d( + SwerveDriveKinematics kinematics, + Rotation2d gyroAngle, + SwerveModulePosition[] modulePositions) { + this(kinematics, gyroAngle, modulePositions, Pose2d.kZero); + } + + /** + * Constructs a SwerveDriveOdometry3d object. + * + * @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. + */ + public SwerveDriveOdometry3d( + SwerveDriveKinematics kinematics, + Rotation3d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose3d initialPose) { + super(kinematics, gyroAngle, modulePositions, initialPose); + + m_numModules = modulePositions.length; + + MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1); + } + + /** + * Constructs a SwerveDriveOdometry3d object with the default pose at the origin. + * + * @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. + */ + public SwerveDriveOdometry3d( + SwerveDriveKinematics kinematics, + Rotation3d gyroAngle, + SwerveModulePosition[] modulePositions) { + this(kinematics, gyroAngle, modulePositions, Pose3d.kZero); + } + + @Override + public void resetPosition( + Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) { + if (modulePositions.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + super.resetPosition(gyroAngle, modulePositions, pose); + } + + @Override + public void resetPosition( + Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d pose) { + if (modulePositions.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + super.resetPosition(gyroAngle, modulePositions, pose); + } + + @Override + public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { + if (modulePositions.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + return super.update(gyroAngle, modulePositions); + } + + @Override + public Pose3d update(Rotation3d gyroAngle, SwerveModulePosition[] modulePositions) { + if (modulePositions.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + return super.update(gyroAngle, modulePositions); + } +} diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp new file mode 100644 index 00000000000..bf81610558f --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp @@ -0,0 +1,44 @@ +// 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. + +#include "frc/estimator/DifferentialDrivePoseEstimator3d.h" + +using namespace frc; + +DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + const Pose2d& initialPose) + : DifferentialDrivePoseEstimator3d{ + kinematics, gyroAngle, leftDistance, rightDistance, + initialPose, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}} {} + +DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + const Pose2d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} + +DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + const Pose3d& initialPose) + : DifferentialDrivePoseEstimator3d{ + kinematics, gyroAngle, leftDistance, + rightDistance, initialPose, {0.02, 0.02, 0.02, 0.01}, + {0.1, 0.1, 0.1, 0.1}} {} + +DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + const Pose3d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp new file mode 100644 index 00000000000..8b3f55d8986 --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp @@ -0,0 +1,46 @@ +// 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. + +#include "frc/estimator/MecanumDrivePoseEstimator3d.h" + +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/AngleStatistics.h" +#include "wpimath/MathShared.h" + +using namespace frc; + +frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) + : MecanumDrivePoseEstimator3d{kinematics, gyroAngle, + wheelPositions, initialPose, + {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}} {} + +frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} + +frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose) + : MecanumDrivePoseEstimator3d{ + kinematics, gyroAngle, + wheelPositions, initialPose, + {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}} {} + +frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} diff --git a/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator3d.cpp new file mode 100644 index 00000000000..7f36471b259 --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator3d.cpp @@ -0,0 +1,12 @@ +// 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. + +#include "frc/estimator/SwerveDrivePoseEstimator3d.h" + +namespace frc { + +template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) + SwerveDrivePoseEstimator3d<4>; + +} // namespace frc diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp new file mode 100644 index 00000000000..22ff24cfd2a --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp @@ -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. + +#include "frc/kinematics/DifferentialDriveOdometry3d.h" + +#include "wpimath/MathShared.h" + +using namespace frc; + +DifferentialDriveOdometry3d::DifferentialDriveOdometry3d( + const Rotation2d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose2d& initialPose) + : Odometry3d( + m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, + initialPose) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); +} + +DifferentialDriveOdometry3d::DifferentialDriveOdometry3d( + const Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose3d& initialPose) + : Odometry3d( + m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, + initialPose) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); +} diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp new file mode 100644 index 00000000000..590ba292d94 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp @@ -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. + +#include "frc/kinematics/MecanumDriveOdometry3d.h" + +#include "wpimath/MathShared.h" + +using namespace frc; + +MecanumDriveOdometry3d::MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) + : Odometry3d( + m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), + m_kinematicsImpl(kinematics) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); +} + +MecanumDriveOdometry3d::MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose) + : Odometry3d( + m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), + m_kinematicsImpl(kinematics) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); +} diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp new file mode 100644 index 00000000000..2325081c0e6 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp @@ -0,0 +1,12 @@ +// 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. + +#include "frc/kinematics/SwerveDriveOdometry3d.h" + +namespace frc { + +template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) + SwerveDriveOdometry3d<4>; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h new file mode 100644 index 00000000000..b0f631d1df5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h @@ -0,0 +1,242 @@ +// 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 +#include + +#include "frc/estimator/PoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveOdometry3d.h" +#include "units/time.h" + +namespace frc { +/** + * This class wraps Differential Drive Odometry to fuse latency-compensated + * vision measurements with differential drive encoder measurements. It will + * correct for noisy vision measurements and encoder drift. It is intended to be + * an easy drop-in for DifferentialDriveOdometry3d. In fact, if you never call + * AddVisionMeasurement(), and only call Update(), this will behave exactly the + * same as DifferentialDriveOdometry3d. + * + * Update() should be called every robot loop (if your robot loops are faster or + * slower than the default of 20 ms, then you should change the nominal delta + * time via the constructor). + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave like regular encoder odometry. + */ +class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d + : public PoseEstimator3d { + public: + /** + * Constructs a DifferentialDrivePoseEstimator3d with default standard + * deviations for the model and vision measurements. + * + * The default standard deviations of the model states are + * 0.02 meters for x, 0.02 meters for y, and 0.01 radians for heading. + * The default standard deviations of the vision measurements are + * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The gyro angle of the robot. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPose The estimated initial pose. + */ + DifferentialDrivePoseEstimator3d(DifferentialDriveKinematics& kinematics, + const Rotation2d& gyroAngle, + units::meter_t leftDistance, + units::meter_t rightDistance, + const Pose2d& initialPose); + + /** + * Constructs a DifferentialDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The gyro angle of the robot. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPose The estimated initial pose. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, and heading in radians). Increase these + * numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + const Pose2d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs); + + /** + * Constructs a DifferentialDrivePoseEstimator3d with default standard + * deviations for the model and vision measurements. + * + * The default standard deviations of the model states are + * 0.02 meters for x, 0.02 meters for y, 0.02 meters for z, and 0.01 radians + * for angle. The default standard deviations of the vision measurements are + * 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for + * angle. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The gyro angle of the robot. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPose The estimated initial pose. + */ + DifferentialDrivePoseEstimator3d(DifferentialDriveKinematics& kinematics, + const Rotation3d& gyroAngle, + units::meter_t leftDistance, + units::meter_t rightDistance, + const Pose3d& initialPose); + + /** + * Constructs a DifferentialDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The gyro angle of the robot. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPose The estimated initial pose. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, z position in meters, and angle in + * radians). Increase these numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, z position in + * meters, and angle in radians). Increase these numbers to trust the vision + * pose measurement less. + */ + DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + const Pose3d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs); + + /** + * Resets the robot's position on the field. + * + * @param gyroAngle The current gyro angle. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param pose The estimated pose of the robot on the field. + */ + void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose2d& pose) { + PoseEstimator3d:: + ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); + } + + /** + * Resets the robot's position on the field. + * + * @param gyroAngle The current gyro angle. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param pose The estimated pose of the robot on the field. + */ + void ResetPosition(const Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose3d& pose) { + PoseEstimator3d:: + ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param gyroAngle The current gyro angle. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * + * @return The estimated pose of the robot. + */ + Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance) { + return PoseEstimator3d< + DifferentialDriveWheelSpeeds, + DifferentialDriveWheelPositions>::Update(gyroAngle, + {leftDistance, rightDistance}); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param gyroAngle The current gyro angle. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * + * @return The estimated pose of the robot. + */ + Pose3d Update(const Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance) { + return PoseEstimator3d< + DifferentialDriveWheelSpeeds, + DifferentialDriveWheelPositions>::Update(gyroAngle, + {leftDistance, rightDistance}); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param currentTime The time at which this method was called. + * @param gyroAngle The current gyro angle. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * + * @return The estimated pose of the robot. + */ + Pose2d UpdateWithTime(units::second_t currentTime, + const Rotation2d& gyroAngle, + units::meter_t leftDistance, + units::meter_t rightDistance) { + return PoseEstimator3d< + DifferentialDriveWheelSpeeds, + DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle, + {leftDistance, + rightDistance}); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param currentTime The time at which this method was called. + * @param gyroAngle The current gyro angle. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * + * @return The estimated pose of the robot. + */ + Pose3d UpdateWithTime(units::second_t currentTime, + const Rotation3d& gyroAngle, + units::meter_t leftDistance, + units::meter_t rightDistance) { + return PoseEstimator3d< + DifferentialDriveWheelSpeeds, + DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle, + {leftDistance, + rightDistance}); + } + + private: + DifferentialDriveOdometry3d m_odometryImpl; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h new file mode 100644 index 00000000000..0b26507301f --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h @@ -0,0 +1,128 @@ +// 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 + +#include +#include + +#include "frc/estimator/PoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/interpolation/TimeInterpolatableBuffer.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/MecanumDriveOdometry3d.h" +#include "units/time.h" + +namespace frc { +/** + * This class wraps Mecanum Drive Odometry to fuse latency-compensated + * vision measurements with mecanum drive encoder velocity measurements. It will + * correct for noisy measurements and encoder drift. It is intended to be an + * easy drop-in for MecanumDriveOdometry3d. + * + * Update() should be called every robot loop. If your loops are faster or + * slower than the default of 20 ms, then you should change the nominal delta + * time by specifying it in the constructor. + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave mostly like regular encoder + * odometry. + */ +class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d + : public PoseEstimator3d { + public: + /** + * Constructs a MecanumDrivePoseEstimator3d with default standard deviations + * for the model and vision measurements. + * + * The default standard deviations of the model states are + * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. + * The default standard deviations of the vision measurements are + * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distance measured by each wheel. + * @param initialPose The starting pose estimate. + */ + MecanumDrivePoseEstimator3d(MecanumDriveKinematics& kinematics, + const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose2d& initialPose); + + /** + * Constructs a MecanumDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distance measured by each wheel. + * @param initialPose The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, and heading in radians). Increase these + * numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose2d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs); + + /** + * Constructs a MecanumDrivePoseEstimator3d with default standard deviations + * for the model and vision measurements. + * + * The default standard deviations of the model states are + * 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for + * angle. The default standard deviations of the vision measurements are 0.45 + * meters for x, 0.45 meters for y, 0.45 meters for z, and 0.45 radians for + * angle. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distance measured by each wheel. + * @param initialPose The starting pose estimate. + */ + MecanumDrivePoseEstimator3d(MecanumDriveKinematics& kinematics, + const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose3d& initialPose); + + /** + * Constructs a MecanumDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distance measured by each wheel. + * @param initialPose The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, z position in meters, and angle in + * radians). Increase these numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, z position in + * meters, and angle in radians). Increase these numbers to trust the vision + * pose measurement less. + */ + MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose3d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs); + + private: + MecanumDriveOdometry3d m_odometryImpl; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h new file mode 100644 index 00000000000..7259df5523b --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -0,0 +1,422 @@ +// 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 +#include +#include +#include + +#include +#include +#include + +#include "frc/EigenCore.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/interpolation/TimeInterpolatableBuffer.h" +#include "frc/kinematics/Kinematics.h" +#include "frc/kinematics/Odometry3d.h" +#include "units/time.h" +#include "wpimath/MathShared.h" + +namespace frc { +/** + * This class wraps odometry to fuse latency-compensated + * vision measurements with encoder measurements. Robot code should not use this + * directly- Instead, use the particular type for your drivetrain (e.g., + * DifferentialDrivePoseEstimator3d). It will correct for noisy vision + * measurements and encoder drift. It is intended to be an easy drop-in for + * Odometry3d. + * + * Update() should be called every robot loop. + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave like regular encoder odometry. + * + * @tparam WheelSpeeds Wheel speeds type. + * @tparam WheelPositions Wheel positions type. + */ +template +class WPILIB_DLLEXPORT PoseEstimator3d { + public: + /** + * Constructs a PoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param odometry A correctly-configured odometry object for your drivetrain. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, and heading in radians). Increase these + * numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + PoseEstimator3d(Kinematics& kinematics, + Odometry3d& odometry, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs); + + /** + * Constructs a PoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param odometry A correctly-configured odometry object for your drivetrain. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, and heading in radians). Increase these + * numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and z position + * in meters, and angle in radians). Increase these numbers to trust the + * vision pose measurement less. + */ + PoseEstimator3d(Kinematics& kinematics, + Odometry3d& odometry, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs); + + /** + * Sets the pose estimator's trust in vision measurements. This might be used + * to change trust in vision measurements after the autonomous period, or to + * change trust as distance to a vision target increases. + * + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + void SetVisionMeasurementStdDevs( + const wpi::array& visionMeasurementStdDevs); + + /** + * Sets the pose estimator's trust in vision measurements. This might be used + * to change trust in vision measurements after the autonomous period, or to + * change trust as distance to a vision target increases. + * + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + void SetVisionMeasurementStdDevs( + const wpi::array& visionMeasurementStdDevs); + + /** + * Resets the robot's position on the field. + * + * 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 gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * @param pose The estimated pose of the robot on the field. + */ + void ResetPosition(const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions, const Pose2d& pose); + + /** + * Resets the robot's position on the field. + * + * 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 gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * @param pose The estimated pose of the robot on the field. + */ + void ResetPosition(const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions, const Pose3d& pose); + + /** + * Resets the robot's pose. + * + * @param pose The pose to reset to. + */ + void ResetPose(const Pose2d& pose); + + /** + * Resets the robot's pose. + * + * @param pose The pose to reset to. + */ + void ResetPose(const Pose3d& pose); + + /** + * Resets the robot's translation. + * + * @param translation The pose to translation to. + */ + void ResetTranslation(const Translation2d& translation); + + /** + * Resets the robot's translation. + * + * @param translation The pose to translation to. + */ + void ResetTranslation(const Translation3d& translation); + + /** + * Resets the robot's rotation. + * + * @param rotation The rotation to reset to. + */ + void ResetRotation(const Rotation2d& rotation); + + /** + * Resets the robot's rotation. + * + * @param rotation The rotation to reset to. + */ + void ResetRotation(const Rotation3d& rotation); + + /** + * Gets the estimated robot pose. + * + * @return The estimated robot pose in meters. + */ + Pose2d GetEstimatedPosition() const; + + /** + * Gets the estimated robot pose. + * + * @return The estimated robot pose in meters. + */ + Pose3d GetEstimatedPosition3d() const; + + /** + * Return the pose at a given timestamp, if the buffer is not empty. + * + * @param timestamp The pose's timestamp. + * @return The pose at the given timestamp (or std::nullopt if the buffer is + * empty). + */ + std::optional SampleAt(units::second_t timestamp) const; + + /** + * Return the pose at a given timestamp, if the buffer is not empty. + * + * @param timestamp The pose's timestamp. + * @return The pose at the given timestamp (or std::nullopt if the buffer is + * empty). + */ + std::optional SampleAt3d(units::second_t timestamp) const; + + /** + * Adds a vision measurement to the Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * To promote stability of the pose estimate and make it robust to bad vision + * data, we recommend only adding vision measurements that are already within + * one meter or so of the current pose estimate. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note + * that if you don't use your own time source by calling UpdateWithTime(), + * then you must use a timestamp with an epoch since FPGA startup (i.e., + * the epoch of this timestamp is the same epoch as + * frc::Timer::GetFPGATimestamp(). This means that you should use + * frc::Timer::GetFPGATimestamp() as your time source in this case. + */ + void AddVisionMeasurement(const Pose2d& visionRobotPose, + units::second_t timestamp); + + /** + * Adds a vision measurement to the Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * To promote stability of the pose estimate and make it robust to bad vision + * data, we recommend only adding vision measurements that are already within + * one meter or so of the current pose estimate. + * + * Note that the vision measurement standard deviations passed into this + * method will continue to apply to future measurements until a subsequent + * call to SetVisionMeasurementStdDevs() or this method. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note + * that if you don't use your own time source by calling UpdateWithTime(), + * then you must use a timestamp with an epoch since FPGA startup (i.e., + * the epoch of this timestamp is the same epoch as + * frc::Timer::GetFPGATimestamp(). This means that you should use + * frc::Timer::GetFPGATimestamp() as your time source in this case. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + void AddVisionMeasurement( + const Pose2d& visionRobotPose, units::second_t timestamp, + const wpi::array& visionMeasurementStdDevs) { + SetVisionMeasurementStdDevs(visionMeasurementStdDevs); + AddVisionMeasurement(visionRobotPose, timestamp); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * To promote stability of the pose estimate and make it robust to bad vision + * data, we recommend only adding vision measurements that are already within + * one meter or so of the current pose estimate. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note + * that if you don't use your own time source by calling UpdateWithTime(), + * then you must use a timestamp with an epoch since FPGA startup (i.e., + * the epoch of this timestamp is the same epoch as + * frc::Timer::GetFPGATimestamp(). This means that you should use + * frc::Timer::GetFPGATimestamp() as your time source in this case. + */ + void AddVisionMeasurement(const Pose3d& visionRobotPose, + units::second_t timestamp); + + /** + * Adds a vision measurement to the Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * To promote stability of the pose estimate and make it robust to bad vision + * data, we recommend only adding vision measurements that are already within + * one meter or so of the current pose estimate. + * + * Note that the vision measurement standard deviations passed into this + * method will continue to apply to future measurements until a subsequent + * call to SetVisionMeasurementStdDevs() or this method. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note + * that if you don't use your own time source by calling UpdateWithTime(), + * then you must use a timestamp with an epoch since FPGA startup (i.e., + * the epoch of this timestamp is the same epoch as + * frc::Timer::GetFPGATimestamp(). This means that you should use + * frc::Timer::GetFPGATimestamp() as your time source in this case. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + void AddVisionMeasurement( + const Pose3d& visionRobotPose, units::second_t timestamp, + const wpi::array& visionMeasurementStdDevs) { + SetVisionMeasurementStdDevs(visionMeasurementStdDevs); + AddVisionMeasurement(visionRobotPose, timestamp); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * + * @return The estimated pose of the robot in meters. + */ + Pose2d Update(const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions); + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * + * @return The estimated pose of the robot in meters. + */ + Pose3d Update(const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions); + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param currentTime The time at which this method was called. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * + * @return The estimated pose of the robot in meters. + */ + Pose2d UpdateWithTime(units::second_t currentTime, + const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions); + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param currentTime The time at which this method was called. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * + * @return The estimated pose of the robot in meters. + */ + Pose3d UpdateWithTime(units::second_t currentTime, + const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions); + + private: + /** + * Removes stale vision updates that won't affect sampling. + */ + void CleanUpVisionUpdates(); + + struct VisionUpdate { + // The vision-compensated pose estimate + Pose3d visionPose; + + // The pose estimated based solely on odometry + Pose3d odometryPose; + + /** + * Returns the vision-compensated version of the pose. Specifically, changes + * the pose from being relative to this record's odometry pose to being + * relative to this record's vision pose. + * + * @param pose The pose to compensate. + * @return The compensated pose. + */ + Pose3d Compensate(const Pose3d& pose) const { + auto delta = pose - odometryPose; + return visionPose + delta; + } + }; + + static constexpr units::second_t kBufferDuration = 1.5_s; + + Odometry3d& m_odometry; + wpi::array m_q{wpi::empty_array}; + frc::Matrixd<6, 6> m_visionK = frc::Matrixd<6, 6>::Zero(); + + // Maps timestamps to odometry-only pose estimates + TimeInterpolatableBuffer m_odometryPoseBuffer{kBufferDuration}; + // Maps timestamps to vision updates + // Always contains one entry before the oldest entry in m_odometryPoseBuffer, + // unless there have been no vision measurements after the last reset + std::map m_visionUpdates; + + Pose3d m_poseEstimate; +}; +} // namespace frc + +#include "frc/estimator/PoseEstimator3d.inc" diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.inc new file mode 100644 index 00000000000..c3865eab4a6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.inc @@ -0,0 +1,339 @@ +// 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 "frc/estimator/PoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Translation2d.h" + +namespace frc { + +template +PoseEstimator3d::PoseEstimator3d( + Kinematics& kinematics, + Odometry3d& odometry, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d{kinematics, + odometry, + {stateStdDevs[0], stateStdDevs[1], 0.0, stateStdDevs[2]}, + {visionMeasurementStdDevs[0], visionMeasurementStdDevs[1], + 0.0, visionMeasurementStdDevs[2]}} {} + +template +PoseEstimator3d::PoseEstimator3d( + Kinematics& kinematics, + Odometry3d& odometry, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : m_odometry(odometry), m_poseEstimate(m_odometry.GetPose3d()) { + for (size_t i = 0; i < 4; ++i) { + m_q[i] = stateStdDevs[i] * stateStdDevs[i]; + } + + SetVisionMeasurementStdDevs(visionMeasurementStdDevs); +} + +template +void PoseEstimator3d::SetVisionMeasurementStdDevs( + const wpi::array& visionMeasurementStdDevs) { + SetVisionMeasurementStdDevs({visionMeasurementStdDevs[0], + visionMeasurementStdDevs[1], 0.0, + visionMeasurementStdDevs[2]}); +} + +template +void PoseEstimator3d::SetVisionMeasurementStdDevs( + const wpi::array& visionMeasurementStdDevs) { + wpi::array r{wpi::empty_array}; + for (size_t i = 0; i < 4; ++i) { + r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i]; + } + + // Solve for closed form Kalman gain for continuous Kalman filter with A = 0 + // and C = I. See wpimath/algorithms.md. + for (size_t row = 0; row < 4; ++row) { + if (m_q[row] == 0.0) { + m_visionK(row, row) = 0.0; + } else { + m_visionK(row, row) = + m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row])); + } + } + double angle_gain = m_visionK(3, 3); + m_visionK(4, 4) = angle_gain; + m_visionK(5, 5) = angle_gain; +} + +template +void PoseEstimator3d::ResetPosition( + const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, + const Pose2d& pose) { + ResetPosition(Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, wheelPositions, + Pose3d{pose}); +} + +template +void PoseEstimator3d::ResetPosition( + const Rotation3d& gyroAngle, const WheelPositions& wheelPositions, + const Pose3d& pose) { + // Reset state estimate and error covariance + m_odometry.ResetPosition(gyroAngle, wheelPositions, pose); + m_odometryPoseBuffer.Clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.GetPose3d(); +} + +template +void PoseEstimator3d::ResetPose( + const Pose2d& pose) { + ResetPose(Pose3d{pose}); +} + +template +void PoseEstimator3d::ResetPose( + const Pose3d& pose) { + m_odometry.ResetPose(pose); + m_odometryPoseBuffer.Clear(); +} + +template +void PoseEstimator3d::ResetTranslation( + const Translation2d& translation) { + ResetTranslation(Translation3d{translation.X(), translation.Y(), 0_m}); +} + +template +void PoseEstimator3d::ResetTranslation( + const Translation3d& translation) { + m_odometry.ResetTranslation(translation); + m_odometryPoseBuffer.Clear(); +} + +template +void PoseEstimator3d::ResetRotation( + const Rotation2d& rotation) { + ResetRotation(Rotation3d{0_rad, 0_rad, rotation.Radians()}); +} + +template +void PoseEstimator3d::ResetRotation( + const Rotation3d& rotation) { + m_odometry.ResetTranslation(rotation); + m_odometryPoseBuffer.Clear(); +} + +template +Pose2d PoseEstimator3d::GetEstimatedPosition() + const { + return GetEstimatedPosition3d().ToPose2d(); +} + +template +Pose3d PoseEstimator3d::GetEstimatedPosition3d() + const { + return m_poseEstimate; + if (m_visionUpdates.empty()) { + return m_odometry.GetPose3d(); + } + auto visionUpdate = m_visionUpdates.rbegin()->second; + return visionUpdate.Compensate(m_odometry.GetPose3d()); +} + +template +std::optional PoseEstimator3d::SampleAt( + units::second_t timestamp) const { + auto sample = SampleAt3d(timestamp); + // TODO Replace with std::optional::transform() in C++23 + if (sample) { + return sample->ToPose2d(); + } else { + return std::nullopt; + } +} + +template +std::optional PoseEstimator3d::SampleAt3d( + units::second_t timestamp) const { + // Step 0: If there are no odometry updates to sample, skip. + if (m_odometryPoseBuffer.GetInternalBuffer().empty()) { + return std::nullopt; + } + + // Step 1: Make sure timestamp matches the sample from the odometry pose + // buffer. (When sampling, the buffer will always use a timestamp + // between the first and last timestamps) + units::second_t oldestOdometryTimestamp = + m_odometryPoseBuffer.GetInternalBuffer().front().first; + units::second_t newestOdometryTimestamp = + m_odometryPoseBuffer.GetInternalBuffer().back().first; + timestamp = + std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp); + + // Step 2: If there are no applicable vision updates, use the odometry-only + // information. + if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) { + return m_odometryPoseBuffer.Sample(timestamp); + } + + // Step 3: Get the latest vision update from before or at the timestamp to + // sample at. + // First, find the iterator past the sample timestamp, then go back one. Note + // that upper_bound() won't return begin() because we check begin() earlier. + auto floorIter = m_visionUpdates.upper_bound(timestamp); + --floorIter; + auto visionUpdate = floorIter->second; + + // Step 4: Get the pose measured by odometry at the time of the sample. + auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp); + + // Step 5: Apply the vision compensation to the odometry pose. + // TODO Replace with std::optional::transform() in C++23 + if (odometryEstimate) { + return visionUpdate.Compensate(*odometryEstimate); + } + return std::nullopt; +} + +template +void PoseEstimator3d::CleanUpVisionUpdates() { + // Step 0: If there are no odometry samples, skip. + if (m_odometryPoseBuffer.GetInternalBuffer().empty()) { + return; + } + + // Step 1: Find the oldest timestamp that needs a vision update. + units::second_t oldestOdometryTimestamp = + m_odometryPoseBuffer.GetInternalBuffer().front().first; + + // Step 2: If there are no vision updates before that timestamp, skip. + if (m_visionUpdates.empty() || + oldestOdometryTimestamp < m_visionUpdates.begin()->first) { + return; + } + + // Step 3: Find the newest vision update timestamp before or at the oldest + // timestamp. + // First, find the iterator past the oldest odometry timestamp, then go + // back one. Note that upper_bound() won't return begin() because we check + // begin() earlier. + auto newestNeededVisionUpdate = + m_visionUpdates.upper_bound(oldestOdometryTimestamp); + --newestNeededVisionUpdate; + + // Step 4: Remove all entries strictly before the newest timestamp we need. + m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate); +} + +template +void PoseEstimator3d::AddVisionMeasurement( + const Pose2d& visionRobotPose, units::second_t timestamp) { + AddVisionMeasurement(Pose3d{visionRobotPose}, timestamp); +} + +template +void PoseEstimator3d::AddVisionMeasurement( + const Pose3d& visionRobotPose, units::second_t timestamp) { + // Step 0: If this measurement is old enough to be outside the pose buffer's + // timespan, skip. + if (m_odometryPoseBuffer.GetInternalBuffer().empty() || + m_odometryPoseBuffer.GetInternalBuffer().front().first - kBufferDuration > + timestamp) { + return; + } + + // Step 1: Clean up any old entries + CleanUpVisionUpdates(); + + // Step 2: Get the pose measured by odometry at the moment the vision + // measurement was made. + auto odometrySample = m_odometryPoseBuffer.Sample(timestamp); + + if (!odometrySample) { + return; + } + + // Step 3: Get the vision-compensated pose estimate at the moment the vision + // measurement was made. + auto visionSample = SampleAt3d(timestamp); + + if (!visionSample) { + return; + } + + // Step 4: Measure the twist between the old pose estimate and the vision + // pose. + auto twist = visionSample.value().Log(visionRobotPose); + + // Step 5: We should not trust the twist entirely, so instead we scale this + // twist by a Kalman gain matrix representing how much we trust vision + // measurements compared to our current pose. + frc::Vectord<6> k_times_twist = + m_visionK * frc::Vectord<6>{twist.dx.value(), twist.dy.value(), + twist.dz.value(), twist.rx.value(), + twist.ry.value(), twist.rz.value()}; + + // Step 6: Convert back to Twist3d. + Twist3d scaledTwist{ + units::meter_t{k_times_twist(0)}, units::meter_t{k_times_twist(1)}, + units::meter_t{k_times_twist(2)}, units::radian_t{k_times_twist(3)}, + units::radian_t{k_times_twist(4)}, units::radian_t{k_times_twist(5)}}; + + // Step 7: Calculate and record the vision update. + VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample}; + m_visionUpdates[timestamp] = visionUpdate; + + // Step 8: Remove later vision measurements. (Matches previous behavior) + auto firstAfter = m_visionUpdates.upper_bound(timestamp); + m_visionUpdates.erase(firstAfter, m_visionUpdates.end()); + + // Step 9: Update latest pose estimate. Since we cleared all updates after + // this vision update, it's guaranteed to be the latest vision update. + m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose3d()); +} + +template +Pose2d PoseEstimator3d::Update( + const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { + return Update(Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, wheelPositions) + .ToPose2d(); +} + +template +Pose3d PoseEstimator3d::Update( + const Rotation3d& gyroAngle, const WheelPositions& wheelPositions) { + return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle, + wheelPositions); +} + +template +Pose2d PoseEstimator3d::UpdateWithTime( + units::second_t currentTime, const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions) { + return UpdateWithTime(currentTime, + Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, + wheelPositions) + .ToPose2d(); +} + +template +Pose3d PoseEstimator3d::UpdateWithTime( + units::second_t currentTime, const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions) { + auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions); + + m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate); + + if (m_visionUpdates.empty()) { + m_poseEstimate = odometryEstimate; + } else { + auto visionUpdate = m_visionUpdates.rbegin()->second; + m_poseEstimate = visionUpdate.Compensate(odometryEstimate); + } + + return GetEstimatedPosition3d(); +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h new file mode 100644 index 00000000000..b5ff93343e0 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h @@ -0,0 +1,152 @@ +// 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 + +#include +#include + +#include "frc/estimator/PoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/kinematics/SwerveDriveOdometry3d.h" +#include "units/time.h" + +namespace frc { + +/** + * This class wraps Swerve Drive Odometry to fuse latency-compensated + * vision measurements with swerve drive encoder distance measurements. It is + * intended to be a drop-in for SwerveDriveOdometry3d. + * + * Update() should be called every robot loop. + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave as regular encoder + * odometry. + */ +template +class SwerveDrivePoseEstimator3d + : public PoseEstimator3d, + wpi::array> { + public: + /** + * Constructs a SwerveDrivePoseEstimator3d with default standard deviations + * for the model and vision measurements. + * + * The default standard deviations of the model states are + * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. + * The default standard deviations of the vision measurements are + * 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance and rotation measurements of + * the swerve modules. + * @param initialPose The starting pose estimate. + */ + SwerveDrivePoseEstimator3d( + SwerveDriveKinematics& kinematics, + const Rotation2d& gyroAngle, + const wpi::array& modulePositions, + const Pose2d& initialPose) + : SwerveDrivePoseEstimator3d{kinematics, gyroAngle, + modulePositions, initialPose, + {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}} {} + + /** + * Constructs a SwerveDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance and rotation measurements of + * the swerve modules. + * @param initialPose The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, and heading in radians). Increase these + * numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + SwerveDrivePoseEstimator3d( + SwerveDriveKinematics& kinematics, + const Rotation2d& gyroAngle, + const wpi::array& modulePositions, + const Pose2d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d, + wpi::array>( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} + + /** + * Constructs a SwerveDrivePoseEstimator3d with default standard deviations + * for the model and vision measurements. + * + * The default standard deviations of the model states are + * 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for + * angle. The default standard deviations of the vision measurements are 0.9 + * meters for x, 0.9 meters for y, 0.9 meters for z, and 0.9 radians for + * angle. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance and rotation measurements of + * the swerve modules. + * @param initialPose The starting pose estimate. + */ + SwerveDrivePoseEstimator3d( + SwerveDriveKinematics& kinematics, + const Rotation3d& gyroAngle, + const wpi::array& modulePositions, + const Pose3d& initialPose) + : SwerveDrivePoseEstimator3d{kinematics, gyroAngle, + modulePositions, initialPose, + {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}} { + } + + /** + * Constructs a SwerveDrivePoseEstimator3d. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param gyroAngle The current gyro angle. + * @param modulePositions The current distance and rotation measurements of + * the swerve modules. + * @param initialPose The starting pose estimate. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, and heading in radians). Increase these + * numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + SwerveDrivePoseEstimator3d( + SwerveDriveKinematics& kinematics, + const Rotation3d& gyroAngle, + const wpi::array& modulePositions, + const Pose3d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d, + wpi::array>( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} + + private: + SwerveDriveOdometry3d m_odometryImpl; +}; + +extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) + SwerveDrivePoseEstimator3d<4>; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h new file mode 100644 index 00000000000..ac998fcf9e9 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h @@ -0,0 +1,152 @@ +// 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 + +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveWheelPositions.h" +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" +#include "frc/kinematics/Odometry3d.h" +#include "units/length.h" + +namespace frc { +/** + * Class for differential drive odometry. Odometry allows you to track the + * robot's position on the field over the course of a match using readings from + * 2 encoders and a gyroscope. + * + * Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + * + * It is important that you reset your encoders to zero before using this class. + * Any subsequent pose resets also require the encoders to be reset to zero. + */ +class WPILIB_DLLEXPORT DifferentialDriveOdometry3d + : public Odometry3d { + public: + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * IF leftDistance and rightDistance are unspecified, + * You NEED to reset your encoders (to zero). + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPose The starting position of the robot on the field. + */ + explicit DifferentialDriveOdometry3d(const Rotation2d& gyroAngle, + units::meter_t leftDistance, + units::meter_t rightDistance, + const Pose2d& initialPose = Pose2d{}); + + /** + * Constructs a DifferentialDriveOdometry3d object. + * + * IF leftDistance and rightDistance are unspecified, + * You NEED to reset your encoders (to zero). + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPose The starting position of the robot on the field. + */ + explicit DifferentialDriveOdometry3d(const Rotation3d& gyroAngle, + units::meter_t leftDistance, + units::meter_t rightDistance, + const Pose3d& initialPose = Pose3d{}); + + /** + * Resets the robot's position on the field. + * + * IF leftDistance and rightDistance are unspecified, + * You NEED to reset your encoders (to zero). + * + * 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 leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + */ + void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose2d& pose) { + Odometry3d::ResetPosition(gyroAngle, + {leftDistance, + rightDistance}, + pose); + } + + /** + * Resets the robot's position on the field. + * + * IF leftDistance and rightDistance are unspecified, + * You NEED to reset your encoders (to zero). + * + * 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 leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + */ + void ResetPosition(const Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose3d& pose) { + Odometry3d::ResetPosition(gyroAngle, + {leftDistance, + rightDistance}, + pose); + } + + /** + * Updates the robot position on the field using distance measurements from + * encoders. This method is more numerically accurate than using velocities to + * integrate the pose and is also advantageous for teams that are using lower + * CPR encoders. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @return The new pose of the robot. + */ + const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance) { + return Odometry3d::Update(gyroAngle, + {leftDistance, + rightDistance}); + } + + /** + * Updates the robot position on the field using distance measurements from + * encoders. This method is more numerically accurate than using velocities to + * integrate the pose and is also advantageous for teams that are using lower + * CPR encoders. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @return The new pose of the robot. + */ + const Pose3d& Update(const Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance) { + return Odometry3d::Update(gyroAngle, + {leftDistance, + rightDistance}); + } + + private: + DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}}; +}; +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h new file mode 100644 index 00000000000..7a961476db4 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h @@ -0,0 +1,61 @@ +// 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 +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/MecanumDriveWheelPositions.h" +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" +#include "frc/kinematics/Odometry3d.h" +#include "units/time.h" + +namespace frc { + +/** + * Class for mecanum drive odometry. Odometry allows you to track the robot's + * position on the field over a course of a match using readings from your + * mecanum wheel encoders. + * + * Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + */ +class WPILIB_DLLEXPORT MecanumDriveOdometry3d + : public Odometry3d { + public: + /** + * Constructs a MecanumDriveOdometry3d object. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * @param initialPose The starting position of the robot on the field. + */ + explicit MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose2d& initialPose = Pose2d{}); + + /** + * Constructs a MecanumDriveOdometry3d object. + * + * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * @param initialPose The starting position of the robot on the field. + */ + explicit MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose3d& initialPose = Pose3d{}); + + private: + MecanumDriveKinematics m_kinematicsImpl; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h new file mode 100644 index 00000000000..2985b2f3513 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h @@ -0,0 +1,205 @@ +// 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 + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Rotation3d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Translation3d.h" +#include "frc/kinematics/Kinematics.h" + +namespace frc { +/** + * Class for odometry. Robot code should not use this directly- Instead, use the + * particular type for your drivetrain (e.g., DifferentialDriveOdometry3d). + * Odometry allows you to track the robot's position on the field over a course + * of a match using readings from your wheel encoders. + * + * Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + * + * @tparam WheelSpeeds Wheel speeds type. + * @tparam WheelPositions Wheel positions type. + */ +template +class WPILIB_DLLEXPORT Odometry3d { + public: + /** + * Constructs an Odometry3d object. + * + * @param kinematics The kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * @param initialPose The starting position of the robot on the field. + */ + explicit Odometry3d(const Kinematics& kinematics, + const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions, + const Pose2d& initialPose = Pose2d{}); + + /** + * Constructs an Odometry3d object. + * + * @param kinematics The kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * @param initialPose The starting position of the robot on the field. + */ + explicit Odometry3d(const Kinematics& kinematics, + const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions, + const Pose3d& initialPose = Pose3d{}); + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * @param pose The position on the field that your robot is at. + */ + void ResetPosition(const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions, const Pose2d& pose) { + ResetPosition(Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, wheelPositions, + Pose3d{pose}); + } + + /** + * 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 gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * @param pose The position on the field that your robot is at. + */ + void ResetPosition(const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions, const Pose3d& pose) { + m_pose = pose; + m_pose2d = m_pose.ToPose2d(); + m_previousAngle = pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; + m_previousWheelPositions = wheelPositions; + } + + /** + * Resets the pose. + * + * @param pose The pose to reset to. + */ + void ResetPose(const Pose2d& pose) { ResetPose(Pose3d{pose}); } + + /** + * Resets the pose. + * + * @param pose The pose to reset to. + */ + void ResetPose(const Pose3d& pose) { + m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation()); + m_pose = pose; + m_pose2d = m_pose.ToPose2d(); + m_previousAngle = pose.Rotation(); + } + + /** + * Resets the translation of the pose. + * + * @param translation The translation to reset to. + */ + void ResetTranslation(const Translation2d& translation) { + ResetTranslation(Translation3d{translation.X(), translation.Y(), 0_m}); + } + + /** + * Resets the translation of the pose. + * + * @param translation The translation to reset to. + */ + void ResetTranslation(const Translation3d& translation) { + m_pose = Pose3d{translation, m_pose.Rotation()}; + m_pose2d = m_pose.ToPose2d(); + } + + /** + * Resets the rotation of the pose. + * + * @param rotation The rotation to reset to. + */ + void ResetRotation(const Rotation2d& rotation) { + ResetRotation(Rotation3d{0_rad, 0_rad, rotation.Radians()}); + } + + /** + * Resets the rotation of the pose. + * + * @param rotation The rotation to reset to. + */ + void ResetRotation(const Rotation3d& rotation) { + m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation()); + m_pose = Pose3d{m_pose.Translation(), rotation}; + m_pose2d = m_pose.ToPose2d(); + m_previousAngle = rotation; + } + + /** + * Returns the position of the robot on the field. + * @return The pose of the robot. + */ + const Pose2d& GetPose() const { return m_pose2d; } + + /** + * Returns the position of the robot on the field. + * @return The pose of the robot. + */ + const Pose3d& GetPose3d() const { return m_pose; } + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method takes in an angle parameter + * which is used instead of the angular rate that is calculated from forward + * kinematics, in addition to the current distance measurement at each wheel. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * + * @return The new pose of the robot. + */ + const Pose2d& Update(const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions); + + /** + * Updates the robot's position on the field using forward kinematics and + * integration of the pose over time. This method takes in an angle parameter + * which is used instead of the angular rate that is calculated from forward + * kinematics, in addition to the current distance measurement at each wheel. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * + * @return The new pose of the robot. + */ + const Pose3d& Update(const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions); + + private: + const Kinematics& m_kinematics; + Pose2d m_pose2d; // Only for GetPose() + Pose3d m_pose; + + WheelPositions m_previousWheelPositions; + Rotation3d m_previousAngle; + Rotation3d m_gyroOffset; +}; +} // namespace frc + +#include "Odometry3d.inc" diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.inc b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.inc new file mode 100644 index 00000000000..f0fb691817c --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.inc @@ -0,0 +1,62 @@ +// 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 "frc/kinematics/Odometry3d.h" + +namespace frc { +template +Odometry3d::Odometry3d( + const Kinematics& kinematics, + const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, + const Pose2d& initialPose) + : Odometry3d{kinematics, Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, + wheelPositions, Pose3d{initialPose}} {} + +template +Odometry3d::Odometry3d( + const Kinematics& kinematics, + const Rotation3d& gyroAngle, const WheelPositions& wheelPositions, + const Pose3d& initialPose) + : m_kinematics(kinematics), + m_pose(initialPose), + m_previousWheelPositions(wheelPositions) { + m_previousAngle = m_pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; +} + +template +const Pose2d& Odometry3d::Update( + const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { + Update(Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, wheelPositions); + return GetPose(); +} + +template +const Pose3d& Odometry3d::Update( + const Rotation3d& gyroAngle, const WheelPositions& wheelPositions) { + auto angle = gyroAngle + m_gyroOffset; + auto angle_difference = + (angle - m_previousAngle).GetQuaternion().ToRotationVector(); + + auto twist2d = + m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions); + Twist3d twist{twist2d.dx, + twist2d.dy, + 0_m, + units::radian_t{angle_difference(0)}, + units::radian_t{angle_difference(1)}, + units::radian_t{angle_difference(2)}}; + + auto newPose = m_pose.Exp(twist); + + m_previousWheelPositions = wheelPositions; + m_previousAngle = angle; + m_pose = {newPose.Translation(), angle}; + m_pose2d = m_pose.ToPose2d(); + + return m_pose; +} +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h new file mode 100644 index 00000000000..6aec6e72bd5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h @@ -0,0 +1,72 @@ +// 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 +#include +#include + +#include +#include + +#include "Odometry3d.h" +#include "SwerveDriveKinematics.h" +#include "SwerveModulePosition.h" +#include "SwerveModuleState.h" +#include "frc/geometry/Pose2d.h" +#include "units/time.h" + +namespace frc { + +/** + * Class for swerve drive odometry. Odometry allows you to track the robot's + * position on the field over a course of a match using readings from your + * swerve drive encoders and swerve azimuth encoders. + * + * Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + */ +template +class SwerveDriveOdometry3d + : public Odometry3d, + wpi::array> { + public: + /** + * Constructs a SwerveDriveOdometry3d object. + * + * @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. + */ + SwerveDriveOdometry3d( + SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, + const wpi::array& modulePositions, + const Pose2d& initialPose = Pose2d{}); + + /** + * Constructs a SwerveDriveOdometry3d object. + * + * @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. + */ + SwerveDriveOdometry3d( + SwerveDriveKinematics kinematics, const Rotation3d& gyroAngle, + const wpi::array& modulePositions, + const Pose3d& initialPose = Pose3d{}); + + private: + SwerveDriveKinematics m_kinematicsImpl; +}; + +extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) + SwerveDriveOdometry3d<4>; + +} // namespace frc + +#include "SwerveDriveOdometry3d.inc" diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.inc new file mode 100644 index 00000000000..f1730df2217 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.inc @@ -0,0 +1,36 @@ +// 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 "frc/kinematics/SwerveDriveOdometry3d.h" +#include "wpimath/MathShared.h" + +namespace frc { +template +SwerveDriveOdometry3d::SwerveDriveOdometry3d( + SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, + const wpi::array& modulePositions, + const Pose2d& initialPose) + : Odometry3d, + wpi::array>( + m_kinematicsImpl, gyroAngle, modulePositions, initialPose), + m_kinematicsImpl(kinematics) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); +} + +template +SwerveDriveOdometry3d::SwerveDriveOdometry3d( + SwerveDriveKinematics kinematics, const Rotation3d& gyroAngle, + const wpi::array& modulePositions, + const Pose3d& initialPose) + : Odometry3d, + wpi::array>( + m_kinematicsImpl, gyroAngle, modulePositions, initialPose), + m_kinematicsImpl(kinematics) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); +} +} // namespace frc From ef808243a2842ab7209840cb8874b1598ec133c3 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 12 Aug 2024 20:49:16 -0700 Subject: [PATCH 2/6] Simplify base class references --- .../DifferentialDrivePoseEstimator.cpp | 5 ++- .../DifferentialDrivePoseEstimator3d.cpp | 10 +++--- .../estimator/MecanumDrivePoseEstimator.cpp | 4 +-- .../estimator/MecanumDrivePoseEstimator3d.cpp | 8 ++--- .../kinematics/DifferentialDriveOdometry.cpp | 5 ++- .../DifferentialDriveOdometry3d.cpp | 10 +++--- .../cpp/kinematics/MecanumDriveOdometry.cpp | 3 +- .../cpp/kinematics/MecanumDriveOdometry3d.cpp | 6 ++-- .../DifferentialDrivePoseEstimator.h | 17 +++------- .../DifferentialDrivePoseEstimator3d.h | 34 ++++++------------- .../frc/estimator/SwerveDrivePoseEstimator.h | 3 +- .../estimator/SwerveDrivePoseEstimator3d.h | 6 ++-- .../kinematics/DifferentialDriveOdometry.h | 11 ++---- .../kinematics/DifferentialDriveOdometry3d.h | 22 +++--------- .../frc/kinematics/SwerveDriveOdometry.inc | 5 ++- .../frc/kinematics/SwerveDriveOdometry3d.inc | 10 +++--- 16 files changed, 51 insertions(+), 108 deletions(-) diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index bf844a12aa8..c349499dfab 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -21,7 +21,6 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + : PoseEstimator(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp index bf81610558f..964a558b912 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp @@ -19,9 +19,8 @@ DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + : PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( @@ -38,7 +37,6 @@ DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( units::meter_t leftDistance, units::meter_t rightDistance, const Pose3d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + : PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 7c75e71ffee..a9ac6d04033 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -24,6 +24,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + : PoseEstimator(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp index 8b3f55d8986..361bc8fa345 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp @@ -24,8 +24,8 @@ frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + : PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( @@ -41,6 +41,6 @@ frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + : PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp index 346a232e75c..d41a1c077f6 100644 --- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp +++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp @@ -11,9 +11,8 @@ using namespace frc; DifferentialDriveOdometry::DifferentialDriveOdometry( const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& initialPose) - : Odometry( - m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, - initialPose) { + : Odometry(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, + initialPose) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); } diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp index 22ff24cfd2a..deea7b90a0f 100644 --- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp +++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp @@ -11,9 +11,8 @@ using namespace frc; DifferentialDriveOdometry3d::DifferentialDriveOdometry3d( const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& initialPose) - : Odometry3d( - m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, - initialPose) { + : Odometry3d(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, + initialPose) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); } @@ -21,9 +20,8 @@ DifferentialDriveOdometry3d::DifferentialDriveOdometry3d( DifferentialDriveOdometry3d::DifferentialDriveOdometry3d( const Rotation3d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose3d& initialPose) - : Odometry3d( - m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, - initialPose) { + : Odometry3d(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, + initialPose) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); } diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp index 55055be744e..f3431e469ec 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp @@ -11,8 +11,7 @@ using namespace frc; MecanumDriveOdometry::MecanumDriveOdometry( MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) - : Odometry( - m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), + : Odometry(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp index 590ba292d94..0e86217d088 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp @@ -11,8 +11,7 @@ using namespace frc; MecanumDriveOdometry3d::MecanumDriveOdometry3d( MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) - : Odometry3d( - m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), + : Odometry3d(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); @@ -21,8 +20,7 @@ MecanumDriveOdometry3d::MecanumDriveOdometry3d( MecanumDriveOdometry3d::MecanumDriveOdometry3d( MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle, const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose) - : Odometry3d( - m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), + : Odometry3d(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 89dcd35cb06..ccd09a5d0a9 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -89,9 +89,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator */ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& pose) { - PoseEstimator:: - ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); + PoseEstimator::ResetPosition(gyroAngle, {leftDistance, rightDistance}, + pose); } /** @@ -106,10 +105,7 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator */ Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return PoseEstimator< - DifferentialDriveWheelSpeeds, - DifferentialDriveWheelPositions>::Update(gyroAngle, - {leftDistance, rightDistance}); + return PoseEstimator::Update(gyroAngle, {leftDistance, rightDistance}); } /** @@ -127,11 +123,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return PoseEstimator< - DifferentialDriveWheelSpeeds, - DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle, - {leftDistance, - rightDistance}); + return PoseEstimator::UpdateWithTime(currentTime, gyroAngle, + {leftDistance, rightDistance}); } private: diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h index b0f631d1df5..7531de41158 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h @@ -135,9 +135,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d */ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& pose) { - PoseEstimator3d:: - ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); + PoseEstimator3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, + pose); } /** @@ -150,9 +149,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d */ void ResetPosition(const Rotation3d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose3d& pose) { - PoseEstimator3d:: - ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); + PoseEstimator3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, + pose); } /** @@ -167,10 +165,7 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d */ Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return PoseEstimator3d< - DifferentialDriveWheelSpeeds, - DifferentialDriveWheelPositions>::Update(gyroAngle, - {leftDistance, rightDistance}); + return PoseEstimator3d::Update(gyroAngle, {leftDistance, rightDistance}); } /** @@ -185,10 +180,7 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d */ Pose3d Update(const Rotation3d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return PoseEstimator3d< - DifferentialDriveWheelSpeeds, - DifferentialDriveWheelPositions>::Update(gyroAngle, - {leftDistance, rightDistance}); + return PoseEstimator3d::Update(gyroAngle, {leftDistance, rightDistance}); } /** @@ -206,11 +198,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return PoseEstimator3d< - DifferentialDriveWheelSpeeds, - DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle, - {leftDistance, - rightDistance}); + return PoseEstimator3d::UpdateWithTime(currentTime, gyroAngle, + {leftDistance, rightDistance}); } /** @@ -228,11 +217,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d const Rotation3d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return PoseEstimator3d< - DifferentialDriveWheelSpeeds, - DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle, - {leftDistance, - rightDistance}); + return PoseEstimator3d::UpdateWithTime(currentTime, gyroAngle, + {leftDistance, rightDistance}); } private: diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 8aeec4e5b3b..1a5081e8f09 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -82,8 +82,7 @@ class SwerveDrivePoseEstimator const wpi::array& modulePositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator, - wpi::array>( + : SwerveDrivePoseEstimator::PoseEstimator( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h index b5ff93343e0..325009ab117 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h @@ -82,8 +82,7 @@ class SwerveDrivePoseEstimator3d const wpi::array& modulePositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d, - wpi::array>( + : SwerveDrivePoseEstimator3d::PoseEstimator3d( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} @@ -137,8 +136,7 @@ class SwerveDrivePoseEstimator3d const wpi::array& modulePositions, const Pose3d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d, - wpi::array>( + : SwerveDrivePoseEstimator3d::PoseEstimator3d( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h index 279c1dcbf21..c0c406c22f1 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -62,11 +62,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry */ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& pose) { - Odometry::ResetPosition(gyroAngle, - {leftDistance, - rightDistance}, - pose); + Odometry::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); } /** @@ -82,10 +78,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry */ const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return Odometry::Update(gyroAngle, - {leftDistance, - rightDistance}); + return Odometry::Update(gyroAngle, {leftDistance, rightDistance}); } private: diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h index ac998fcf9e9..6784ec0070c 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h @@ -78,11 +78,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d */ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& pose) { - Odometry3d::ResetPosition(gyroAngle, - {leftDistance, - rightDistance}, - pose); + Odometry3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); } /** @@ -101,11 +97,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d */ void ResetPosition(const Rotation3d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose3d& pose) { - Odometry3d::ResetPosition(gyroAngle, - {leftDistance, - rightDistance}, - pose); + Odometry3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); } /** @@ -121,10 +113,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d */ const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return Odometry3d::Update(gyroAngle, - {leftDistance, - rightDistance}); + return Odometry3d::Update(gyroAngle, {leftDistance, rightDistance}); } /** @@ -140,10 +129,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d */ const Pose3d& Update(const Rotation3d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return Odometry3d::Update(gyroAngle, - {leftDistance, - rightDistance}); + return Odometry3d::Update(gyroAngle, {leftDistance, rightDistance}); } private: diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc index 2b047c21944..fcbe78e48b8 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc @@ -13,9 +13,8 @@ SwerveDriveOdometry::SwerveDriveOdometry( SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, const wpi::array& modulePositions, const Pose2d& initialPose) - : Odometry, - wpi::array>( - m_kinematicsImpl, gyroAngle, modulePositions, initialPose), + : SwerveDriveOdometry::Odometry(m_kinematicsImpl, gyroAngle, + modulePositions, initialPose), m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.inc index f1730df2217..0e31e859faa 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.inc @@ -13,9 +13,8 @@ SwerveDriveOdometry3d::SwerveDriveOdometry3d( SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, const wpi::array& modulePositions, const Pose2d& initialPose) - : Odometry3d, - wpi::array>( - m_kinematicsImpl, gyroAngle, modulePositions, initialPose), + : SwerveDriveOdometry3d::Odometry3d(m_kinematicsImpl, gyroAngle, + modulePositions, initialPose), m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); @@ -26,9 +25,8 @@ SwerveDriveOdometry3d::SwerveDriveOdometry3d( SwerveDriveKinematics kinematics, const Rotation3d& gyroAngle, const wpi::array& modulePositions, const Pose3d& initialPose) - : Odometry3d, - wpi::array>( - m_kinematicsImpl, gyroAngle, modulePositions, initialPose), + : SwerveDriveOdometry3d::Odometry3d(m_kinematicsImpl, gyroAngle, + modulePositions, initialPose), m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); From 740236aa985a9c6bf102d53ee554c0f6809d16f0 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 12 Aug 2024 21:31:38 -0700 Subject: [PATCH 3/6] Clean up Javadoc link references --- .../edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java | 2 +- .../wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java | 2 +- .../edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java | 2 +- .../wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index 59bbb3235cd..e33b30d09ca 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -18,7 +18,7 @@ * This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated * vision measurements with mecanum drive encoder distance measurements. It will correct for noisy * measurements and encoder drift. It is intended to be a drop-in replacement for {@link - * edu.wpi.first.math.kinematics.MecanumDriveOdometry}. + * MecanumDriveOdometry}. * *

{@link MecanumDrivePoseEstimator#update} should be called every robot loop. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java index b852f31004c..b165246c5ef 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java @@ -21,7 +21,7 @@ * This class wraps {@link MecanumDriveOdometry3d Mecanum Drive Odometry} to fuse * latency-compensated vision measurements with mecanum drive encoder distance measurements. It will * correct for noisy measurements and encoder drift. It is intended to be a drop-in replacement for - * {@link edu.wpi.first.math.kinematics.MecanumDriveOdometry3d}. + * {@link MecanumDriveOdometry3d}. * *

{@link MecanumDrivePoseEstimator3d#update} should be called every robot loop. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 42901670045..495cd893fc6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -17,7 +17,7 @@ /** * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated * vision measurements with swerve drive encoder distance measurements. It is intended to be a - * drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}. + * drop-in replacement for {@link SwerveDriveOdometry}. * *

{@link SwerveDrivePoseEstimator#update} should be called every robot loop. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java index bbb0f92cb2f..2b26b6c9907 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java @@ -20,7 +20,7 @@ /** * This class wraps {@link SwerveDriveOdometry3d Swerve Drive Odometry} to fuse latency-compensated * vision measurements with swerve drive encoder distance measurements. It is intended to be a - * drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry3d}. + * drop-in replacement for {@link SwerveDriveOdometry3d}. * *

{@link SwerveDrivePoseEstimator3d#update} should be called every robot loop. * From 1aabf02d2e8c3ff495aebe91798d6f9231b24321 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 23 Sep 2024 20:13:25 -0700 Subject: [PATCH 4/6] Update DifferentialDriveOdometry3d for new Java units --- .../DifferentialDriveOdometry3d.java | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java index 167338951c6..be505c996cf 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java @@ -12,8 +12,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.units.Distance; -import edu.wpi.first.units.Measure; +import edu.wpi.first.units.measure.Distance; /** * Class for differential drive odometry. Odometry allows you to track the robot's position on the @@ -57,8 +56,8 @@ public DifferentialDriveOdometry3d( */ public DifferentialDriveOdometry3d( Rotation2d gyroAngle, - Measure leftDistance, - Measure rightDistance, + Distance leftDistance, + Distance rightDistance, Pose2d initialPoseMeters) { this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters); } @@ -83,7 +82,7 @@ public DifferentialDriveOdometry3d( * @param rightDistance The distance traveled by the right encoder. */ public DifferentialDriveOdometry3d( - Rotation2d gyroAngle, Measure leftDistance, Measure rightDistance) { + Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance) { this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero); } @@ -118,8 +117,8 @@ public DifferentialDriveOdometry3d( */ public DifferentialDriveOdometry3d( Rotation3d gyroAngle, - Measure leftDistance, - Measure rightDistance, + Distance leftDistance, + Distance rightDistance, Pose3d initialPoseMeters) { this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters); } @@ -144,7 +143,7 @@ public DifferentialDriveOdometry3d( * @param rightDistance The distance traveled by the right encoder. */ public DifferentialDriveOdometry3d( - Rotation3d gyroAngle, Measure leftDistance, Measure rightDistance) { + Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance) { this(gyroAngle, leftDistance, rightDistance, Pose3d.kZero); } @@ -183,8 +182,8 @@ public void resetPosition( */ public void resetPosition( Rotation2d gyroAngle, - Measure leftDistance, - Measure rightDistance, + Distance leftDistance, + Distance rightDistance, Pose2d poseMeters) { resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); } @@ -224,8 +223,8 @@ public void resetPosition( */ public void resetPosition( Rotation3d gyroAngle, - Measure leftDistance, - Measure rightDistance, + Distance leftDistance, + Distance rightDistance, Pose3d poseMeters) { resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); } From ec3c46d21f52b07ecd955ee2a49af1b4bf038b74 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Mon, 23 Sep 2024 20:23:23 -0700 Subject: [PATCH 5/6] Run spotless --- .../math/kinematics/DifferentialDriveOdometry3d.java | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java index be505c996cf..78220aeb96b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java @@ -181,10 +181,7 @@ public void resetPosition( * @param poseMeters The position on the field that your robot is at. */ public void resetPosition( - Rotation2d gyroAngle, - Distance leftDistance, - Distance rightDistance, - Pose2d poseMeters) { + Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance, Pose2d poseMeters) { resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); } @@ -222,10 +219,7 @@ public void resetPosition( * @param poseMeters The position on the field that your robot is at. */ public void resetPosition( - Rotation3d gyroAngle, - Distance leftDistance, - Distance rightDistance, - Pose3d poseMeters) { + Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance, Pose3d poseMeters) { resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); } From 6a13f34cbac2fd7878fbd80b466aaf93b5d79a9f Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Tue, 24 Sep 2024 20:47:25 -0700 Subject: [PATCH 6/6] Add timing code --- wpimath/build.gradle | 2 + .../java/edu/wpi/first/math/TimeTest.java | 146 +++++++++++++ wpimath/src/test/native/cpp/TimeTest.cpp | 191 ++++++++++++++++++ 3 files changed, 339 insertions(+) create mode 100644 wpimath/src/test/java/edu/wpi/first/math/TimeTest.java create mode 100644 wpimath/src/test/native/cpp/TimeTest.cpp diff --git a/wpimath/build.gradle b/wpimath/build.gradle index 388923586a6..dbeadcfa74c 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -118,3 +118,5 @@ task unitsHeaders(type: Zip) { } addTaskToCopyAllOutputs(unitsHeaders) + +test.testLogging.showStandardStreams = true diff --git a/wpimath/src/test/java/edu/wpi/first/math/TimeTest.java b/wpimath/src/test/java/edu/wpi/first/math/TimeTest.java new file mode 100644 index 00000000000..0f98f15dcfe --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/TimeTest.java @@ -0,0 +1,146 @@ +// 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. + +package edu.wpi.first.math; + +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator3d; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry3d; +import java.util.Arrays; +import org.junit.jupiter.api.Test; + +class TimeTest { + @SuppressWarnings("PMD.AvoidArrayLoops") + private static void processDurations(long[] durations, String prefix) { + long total_duration = 0; + for (long duration : durations) { + total_duration += duration; + } + + double mean = (double) total_duration / durations.length; + + double sum_squares = 0; + for (double duration : durations) { + sum_squares += (duration - mean) * (duration - mean); + } + + double std_dev = Math.sqrt(sum_squares / durations.length); + + System.out.println(prefix + "Mean: " + mean + ", Std dev: " + std_dev); + + long[] buffer = new long[10]; + + for (int i = 0; i < 10; ++i) { + buffer[i] = durations[i]; + } + + System.out.println(prefix + "First 10: " + Arrays.toString(buffer)); + + for (int i = 0; i < 10; ++i) { + buffer[i] = durations[durations.length - 10 + i]; + } + + System.out.println(prefix + "Last 10: " + Arrays.toString(buffer)); + + long[] sorted = Arrays.copyOf(durations, durations.length); + Arrays.sort(sorted); + + for (int i = 0; i < 10; ++i) { + buffer[i] = sorted[i]; + } + + System.out.println(prefix + "Fastest 10: " + Arrays.toString(buffer)); + + for (int i = 0; i < 10; ++i) { + buffer[i] = sorted[sorted.length - 10 + i]; + } + + System.out.println(prefix + "Slowest 10: " + Arrays.toString(buffer)); + } + + private static void time(int count, Runnable action, Runnable setup, String prefix) { + long[] durations = new long[count]; + + for (int i = 0; i < count; ++i) { + setup.run(); + long start = System.nanoTime(); + action.run(); + long end = System.nanoTime(); + durations[i] = end - start; + } + + processDurations(durations, prefix); + } + + void timeSuite(String name, Runnable action, Runnable setup) { + System.out.println(name + ":"); + System.out.println(" Warmup: (100,000 iterations):"); + time(100_000, action, setup, " "); + for (int i = 0; i < 5; ++i) { + System.out.println(" Run " + i + ":"); + time(1_000, action, setup, " "); + } + } + + void timeSuite(String name, Runnable action) { + timeSuite(name, action, () -> {}); + } + + @Test + void testTime() { + { + var odometry = new DifferentialDriveOdometry(Rotation2d.kZero, 0, 0, Pose2d.kZero); + var gyroAngle = Rotation2d.kZero; + double leftDistance = 0; + double rightDistance = 0; + timeSuite( + "Odometry update (2d)", + () -> odometry.update(gyroAngle, leftDistance, rightDistance), + () -> odometry.resetPosition(Rotation2d.kZero, 0, 0, Pose2d.kZero)); + } + + { + var odometry = new DifferentialDriveOdometry3d(Rotation3d.kZero, 0, 0, Pose3d.kZero); + var gyroAngle = Rotation3d.kZero; + double leftDistance = 0; + double rightDistance = 0; + timeSuite( + "Odometry update (3d)", + () -> odometry.update(gyroAngle, leftDistance, rightDistance), + () -> odometry.resetPosition(Rotation3d.kZero, 0, 0, Pose3d.kZero)); + } + + { + var kinematics = new DifferentialDriveKinematics(1); + var poseEstimator = + new DifferentialDrivePoseEstimator(kinematics, Rotation2d.kZero, 0, 0, Pose2d.kZero); + var gyroAngle = Rotation2d.kZero; + double leftDistance = 0; + double rightDistance = 0; + timeSuite( + "Pose estimator update (2d)", + () -> poseEstimator.update(gyroAngle, leftDistance, rightDistance), + () -> poseEstimator.resetPosition(Rotation2d.kZero, 0, 0, Pose2d.kZero)); + } + + { + var kinematics = new DifferentialDriveKinematics(1); + var poseEstimator = + new DifferentialDrivePoseEstimator3d(kinematics, Rotation3d.kZero, 0, 0, Pose3d.kZero); + var gyroAngle = Rotation3d.kZero; + double leftDistance = 0; + double rightDistance = 0; + timeSuite( + "Pose estimator update (3d)", + () -> poseEstimator.update(gyroAngle, leftDistance, rightDistance), + () -> poseEstimator.resetPosition(Rotation3d.kZero, 0, 0, Pose3d.kZero)); + } + } +} diff --git a/wpimath/src/test/native/cpp/TimeTest.cpp b/wpimath/src/test/native/cpp/TimeTest.cpp new file mode 100644 index 00000000000..53f85bb909c --- /dev/null +++ b/wpimath/src/test/native/cpp/TimeTest.cpp @@ -0,0 +1,191 @@ +// 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. + +#include +#include + +#include +#include +#include + +#include "frc/estimator/DifferentialDrivePoseEstimator.h" +#include "frc/estimator/DifferentialDrivePoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Rotation3d.h" +#include "frc/kinematics/DifferentialDriveOdometry.h" +#include "frc/kinematics/DifferentialDriveOdometry3d.h" +#include "units/length.h" +#include "units/math.h" +#include "units/time.h" + +template +struct fmt::formatter, CharT> { + template + constexpr auto parse(ParseContext& ctx) { + return m_underlying.parse(ctx); + } + + template + auto format(const wpi::array& arr, FmtContext& ctx) const { + auto out = ctx.out(); + + out = fmt::format_to(out, "["); + + for (size_t i = 0; i < N; ++i) { + out = m_underlying.format(arr[i], ctx); + if (i < N - 1) { + out = fmt::format_to(out, ", "); + } + } + + out = fmt::format_to(out, "]"); + + return out; + } + + private: + fmt::formatter m_underlying; +}; + +template +void ProcessDurations(const wpi::array& durations, + std::string_view prefix = "") { + units::nanosecond_t total_duration = 0_ns; + for (auto duration : durations) { + total_duration += duration; + } + + units::nanosecond_t mean = total_duration / N; + + auto sum_squares = 0_ns * 0_ns; + for (auto duration : durations) { + sum_squares += (duration - mean) * (duration - mean); + } + + units::nanosecond_t std_dev = units::math::sqrt(sum_squares / N); + + wpi::print("{}Mean: {}, Std dev: {}\n", prefix, mean, std_dev); + + wpi::array buffer{wpi::empty_array}; + + for (size_t i = 0; i < 10; ++i) { + buffer[i] = durations[i]; + } + + wpi::print("{}First 10: {}\n", prefix, buffer); + + for (size_t i = 0; i < 10; ++i) { + buffer[i] = durations[N - 10 + i]; + } + + wpi::print("{}Last 10: {}\n", prefix, buffer); + + wpi::array sorted{durations}; + std::sort(sorted.begin(), sorted.end()); + + for (size_t i = 0; i < 10; ++i) { + buffer[i] = sorted[i]; + } + + wpi::print("{}Fastest 10: {}\n", prefix, buffer); + + for (size_t i = 0; i < 10; ++i) { + buffer[i] = sorted[N - 10 + i]; + } + + wpi::print("{}Slowest 10: {}\n", prefix, buffer); +} + +template +void Time( + std::function action, std::function setup = [] {}, + std::string_view prefix = "") { + wpi::array durations(wpi::empty_array); + + for (size_t i = 0; i < N; ++i) { + setup(); + auto start = std::chrono::steady_clock::now(); + action(); + auto end = std::chrono::steady_clock::now(); + durations[i] = end - start; + } + + ProcessDurations(durations, prefix); +} + +void TimeSuite( + std::string_view name, std::function action, + std::function setup = [] {}) { + fmt::print("{}:\n", name); + fmt::print(" Warmup: (100,000 iterations):\n"); + Time<100'000>(action, setup, " "); + for (size_t i = 0; i < 5; ++i) { + fmt::print(" Run {}:\n", i); + Time<1'000>(action, setup, " "); + } +} + +TEST(TimeTest, Time) { + { + frc::DifferentialDriveOdometry odometry{frc::Rotation2d{}, 0_m, 0_m, + frc::Pose2d{}}; + frc::Rotation2d gyroAngle{}; + auto leftDistance = 0_m; + auto rightDistance = 0_m; + TimeSuite( + "Odometry update (2d)", + [&] { odometry.Update(gyroAngle, leftDistance, rightDistance); }, + [&] { + odometry.ResetPosition(frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}); + }); + } + + { + frc::DifferentialDriveOdometry3d odometry{frc::Rotation3d{}, 0_m, 0_m, + frc::Pose3d{}}; + frc::Rotation3d gyroAngle{}; + auto leftDistance = 0_m; + auto rightDistance = 0_m; + TimeSuite( + "Odometry update (3d)", + [&] { odometry.Update(gyroAngle, leftDistance, rightDistance); }, + [&] { + odometry.ResetPosition(frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{}); + }); + } + + { + frc::DifferentialDriveKinematics kinematics{1_m}; + frc::DifferentialDrivePoseEstimator poseEstimator{ + kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}}; + frc::Rotation2d gyroAngle{}; + auto leftDistance = 0_m; + auto rightDistance = 0_m; + TimeSuite( + "Pose estimator update (2d)", + [&] { poseEstimator.Update(gyroAngle, leftDistance, rightDistance); }, + [&] { + poseEstimator.ResetPosition(frc::Rotation2d{}, 0_m, 0_m, + frc::Pose2d{}); + }); + } + + { + frc::DifferentialDriveKinematics kinematics{1_m}; + frc::DifferentialDrivePoseEstimator3d poseEstimator{ + kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{}}; + frc::Rotation3d gyroAngle{}; + auto leftDistance = 0_m; + auto rightDistance = 0_m; + TimeSuite( + "Pose estimator update (3d)", + [&] { poseEstimator.Update(gyroAngle, leftDistance, rightDistance); }, + [&] { + poseEstimator.ResetPosition(frc::Rotation3d{}, 0_m, 0_m, + frc::Pose3d{}); + }); + } +}