diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 141844e362..df0f1ea838 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry; -import edu.wpi.first.math.kinematics.WheelPositions; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import java.util.Map; @@ -38,7 +37,7 @@ import java.util.Optional; * * @param Wheel positions type. */ -public class PoseEstimator> { +public class PoseEstimator { private final Kinematics m_kinematics; private final Odometry m_odometry; private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); @@ -150,11 +149,10 @@ public class PoseEstimator> { * @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 - * PoseEstimator#updateWithTime(double,Rotation2d,WheelPositions)} 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. + * PoseEstimator#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) { // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. @@ -263,7 +261,8 @@ public class PoseEstimator> { m_odometry.update(gyroAngle, wheelPositions); m_poseBuffer.addSample( currentTimeSeconds, - new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy())); + new InterpolationRecord( + getEstimatedPosition(), gyroAngle, m_kinematics.copy(wheelPositions))); return getEstimatedPosition(); } @@ -311,7 +310,7 @@ public class PoseEstimator> { return endValue; } else { // Find the new wheel distances. - var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t); + var wheelLerp = m_kinematics.interpolate(wheelPositions, endValue.wheelPositions, t); // Find the new gyro angle. var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t); 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 f778b621e3..4290167004 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 @@ -10,7 +10,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -25,7 +24,7 @@ import edu.wpi.first.math.numbers.N3; *

{@link SwerveDrivePoseEstimator#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 SwerveDrivePoseEstimator extends PoseEstimator { +public class SwerveDrivePoseEstimator extends PoseEstimator { private final int m_numModules; /** @@ -85,52 +84,10 @@ public class SwerveDrivePoseEstimator extends PoseEstimatorThe 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 angle reported by the gyroscope. - * @param modulePositions The current distance measurements and rotations of the swerve modules. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition( - Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) { - resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters); - } - - /** - * Updates the pose estimator with wheel encoder and gyro information. This should be called every - * loop. - * - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance measurements and rotations of the swerve modules. - * @return The estimated pose of the robot in meters. - */ - public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions)); - } - - /** - * 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 gyroscope angle. - * @param modulePositions The current distance measurements and rotations of the swerve modules. - * @return The estimated pose of the robot in meters. - */ - public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - return updateWithTime( - currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions)); - } - @Override public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) { - if (wheelPositions.positions.length != m_numModules) { + 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"); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java index 1d18b005da..8bcb6a74cf 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -8,6 +8,7 @@ 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.MathUtil; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto; import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct; @@ -113,4 +114,19 @@ public class DifferentialDriveKinematics 0, (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters); } + + @Override + public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions positions) { + return new DifferentialDriveWheelPositions(positions.leftMeters, positions.rightMeters); + } + + @Override + public DifferentialDriveWheelPositions interpolate( + DifferentialDriveWheelPositions startValue, + DifferentialDriveWheelPositions endValue, + double t) { + return new DifferentialDriveWheelPositions( + MathUtil.interpolate(startValue.leftMeters, endValue.leftMeters, t), + MathUtil.interpolate(startValue.rightMeters, endValue.rightMeters, t)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java index 35e2642101..a64c9a2009 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java @@ -5,6 +5,7 @@ package edu.wpi.first.math.kinematics; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.interpolation.Interpolator; /** * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot @@ -14,7 +15,7 @@ import edu.wpi.first.math.geometry.Twist2d; * @param The type of the wheel speeds. * @param

The type of the wheel positions. */ -public interface Kinematics { +public interface Kinematics extends Interpolator

{ /** * Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This * method is often used for odometry -- determining the robot's position on the field using data @@ -44,4 +45,12 @@ public interface Kinematics { * @return The resulting Twist2d in the robot's movement. */ Twist2d toTwist2d(P start, P end); + + /** + * Returns a copy of the wheel positions object. + * + * @param positions The wheel positions object to copy. + * @return A copy. + */ + P copy(P positions); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index f638b14b7c..4589a44853 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -6,6 +6,7 @@ package edu.wpi.first.math.kinematics; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.proto.MecanumDriveKinematicsProto; @@ -255,4 +256,23 @@ public class MecanumDriveKinematics public Translation2d getRearRight() { return m_rearRightWheelMeters; } + + @Override + public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions) { + return new MecanumDriveWheelPositions( + positions.frontLeftMeters, + positions.frontRightMeters, + positions.rearLeftMeters, + positions.rearRightMeters); + } + + @Override + public MecanumDriveWheelPositions interpolate( + MecanumDriveWheelPositions startValue, MecanumDriveWheelPositions endValue, double t) { + return new MecanumDriveWheelPositions( + MathUtil.interpolate(startValue.frontLeftMeters, endValue.frontLeftMeters, t), + MathUtil.interpolate(startValue.frontRightMeters, endValue.frontRightMeters, t), + MathUtil.interpolate(startValue.rearLeftMeters, endValue.rearLeftMeters, t), + MathUtil.interpolate(startValue.rearRightMeters, endValue.rearRightMeters, t)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java index 427c92d8a1..0b4dab7a95 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java @@ -18,7 +18,7 @@ import edu.wpi.first.math.geometry.Rotation2d; * * @param Wheel positions type. */ -public class Odometry> { +public class Odometry { private final Kinematics m_kinematics; private Pose2d m_poseMeters; @@ -43,7 +43,7 @@ public class Odometry> { m_poseMeters = initialPoseMeters; m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_previousAngle = m_poseMeters.getRotation(); - m_previousWheelPositions = wheelPositions.copy(); + m_previousWheelPositions = m_kinematics.copy(wheelPositions); } /** @@ -60,7 +60,7 @@ public class Odometry> { m_poseMeters = poseMeters; m_previousAngle = m_poseMeters.getRotation(); m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - m_previousWheelPositions = wheelPositions.copy(); + m_previousWheelPositions = m_kinematics.copy(wheelPositions); } /** @@ -90,7 +90,7 @@ public class Odometry> { var newPose = m_poseMeters.exp(twist); - m_previousWheelPositions = wheelPositions.copy(); + m_previousWheelPositions = m_kinematics.copy(wheelPositions); m_previousAngle = angle; m_poseMeters = new Pose2d(newPose.getTranslation(), angle); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 9e78094268..39306d0ac6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -39,26 +39,9 @@ import org.ejml.simple.SimpleMatrix; *

Forward kinematics is also used for odometry -- determining the position of the robot on the * field using encoders and a gyro. */ +@SuppressWarnings("overrides") public class SwerveDriveKinematics - implements Kinematics { - /** Wrapper class for swerve module states. */ - public static class SwerveDriveWheelStates { - /** Swerve module states. */ - public SwerveModuleState[] states; - - /** - * Creates a new SwerveDriveWheelStates instance. - * - * @param states The swerve module states. This will be deeply copied. - */ - public SwerveDriveWheelStates(SwerveModuleState[] states) { - this.states = new SwerveModuleState[states.length]; - for (int i = 0; i < states.length; i++) { - this.states[i] = new SwerveModuleState(states[i].speedMetersPerSecond, states[i].angle); - } - } - } - + implements Kinematics { private final SimpleMatrix m_inverseKinematics; private final SimpleMatrix m_forwardKinematics; @@ -200,8 +183,8 @@ public class SwerveDriveKinematics } @Override - public SwerveDriveWheelStates toWheelSpeeds(ChassisSpeeds chassisSpeeds) { - return new SwerveDriveWheelStates(toSwerveModuleStates(chassisSpeeds)); + public SwerveModuleState[] toWheelSpeeds(ChassisSpeeds chassisSpeeds) { + return toSwerveModuleStates(chassisSpeeds); } /** @@ -214,6 +197,7 @@ public class SwerveDriveKinematics * passed into the constructor of this class. * @return The resulting chassis speed. */ + @Override public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) { if (moduleStates.length != m_numModules) { throw new IllegalArgumentException( @@ -235,11 +219,6 @@ public class SwerveDriveKinematics chassisSpeedsVector.get(2, 0)); } - @Override - public ChassisSpeeds toChassisSpeeds(SwerveDriveWheelStates wheelStates) { - return toChassisSpeeds(wheelStates.states); - } - /** * Performs forward kinematics to return the resulting chassis state from the given module states. * This method is often used for odometry -- determining the robot's position on the field using @@ -270,17 +249,14 @@ public class SwerveDriveKinematics } @Override - public Twist2d toTwist2d(SwerveDriveWheelPositions start, SwerveDriveWheelPositions end) { - if (start.positions.length != end.positions.length) { + public Twist2d toTwist2d(SwerveModulePosition[] start, SwerveModulePosition[] end) { + if (start.length != end.length) { throw new IllegalArgumentException("Inconsistent number of modules!"); } - var newPositions = new SwerveModulePosition[start.positions.length]; - for (int i = 0; i < start.positions.length; i++) { - var startModule = start.positions[i]; - var endModule = end.positions[i]; + var newPositions = new SwerveModulePosition[start.length]; + for (int i = 0; i < start.length; i++) { newPositions[i] = - new SwerveModulePosition( - endModule.distanceMeters - startModule.distanceMeters, endModule.angle); + new SwerveModulePosition(end[i].distanceMeters - start[i].distanceMeters, end[i].angle); } return toTwist2d(newPositions); } @@ -406,4 +382,26 @@ public class SwerveDriveKinematics attainableMaxTranslationalSpeed.in(MetersPerSecond), attainableMaxRotationalVelocity.in(RadiansPerSecond)); } + + @Override + public SwerveModulePosition[] copy(SwerveModulePosition[] positions) { + var newPositions = new SwerveModulePosition[positions.length]; + for (int i = 0; i < positions.length; ++i) { + newPositions[i] = positions[i].copy(); + } + return newPositions; + } + + @Override + public SwerveModulePosition[] interpolate( + SwerveModulePosition[] startValue, SwerveModulePosition[] endValue, double t) { + if (endValue.length != startValue.length) { + throw new IllegalArgumentException("Inconsistent number of modules!"); + } + var newPositions = new SwerveModulePosition[startValue.length]; + for (int i = 0; i < startValue.length; ++i) { + newPositions[i] = startValue[i].interpolate(endValue[i], t); + } + return newPositions; + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java index 14f62b2f20..abb6e9739c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java @@ -17,7 +17,7 @@ import edu.wpi.first.math.geometry.Rotation2d; *

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 SwerveDriveOdometry extends Odometry { +public class SwerveDriveOdometry extends Odometry { private final int m_numModules; /** @@ -33,7 +33,7 @@ public class SwerveDriveOdometry extends Odometry { Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d initialPose) { - super(kinematics, gyroAngle, new SwerveDriveWheelPositions(modulePositions), initialPose); + super(kinematics, gyroAngle, modulePositions, initialPose); m_numModules = modulePositions.length; @@ -54,27 +54,10 @@ public class SwerveDriveOdometry extends Odometry { this(kinematics, gyroAngle, modulePositions, Pose2d.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. - * - *

Similarly, module positions do not need to be reset in user code. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param modulePositions The wheel positions reported by each module., - * @param pose The position on the field that your robot is at. - */ - public void resetPosition( - Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) { - resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose); - } - @Override public void resetPosition( - Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) { - if (modulePositions.positions.length != m_numModules) { + 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"); @@ -82,25 +65,9 @@ public class SwerveDriveOdometry extends Odometry { super.resetPosition(gyroAngle, modulePositions, pose); } - /** - * Updates the robot's position on the field using forward kinematics and integration of the pose - * over time. This method automatically calculates the current time to calculate period - * (difference between two timestamps). The period is used to calculate the change in distance - * from a velocity. This also takes in an angle parameter which is used instead of the angular - * rate that is calculated from forward kinematics. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param modulePositions The current position of all swerve modules. Please provide the positions - * in the same order in which you instantiated your SwerveDriveKinematics. - * @return The new pose of the robot. - */ - public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions)); - } - @Override - public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) { - if (modulePositions.positions.length != m_numModules) { + 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"); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java deleted file mode 100644 index 5d0f0af113..0000000000 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java +++ /dev/null @@ -1,63 +0,0 @@ -// 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 java.util.Arrays; -import java.util.Objects; - -/** Represents the wheel positions for a swerve drive drivetrain. */ -public class SwerveDriveWheelPositions implements WheelPositions { - /** The distances driven by the wheels. */ - public SwerveModulePosition[] positions; - - /** - * Creates a new SwerveDriveWheelPositions instance. - * - * @param positions The swerve module positions. This will be deeply copied. - */ - public SwerveDriveWheelPositions(SwerveModulePosition[] positions) { - this.positions = new SwerveModulePosition[positions.length]; - for (int i = 0; i < positions.length; i++) { - this.positions[i] = positions[i].copy(); - } - } - - @Override - public boolean equals(Object obj) { - if (obj instanceof SwerveDriveWheelPositions) { - SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj; - return Arrays.equals(this.positions, other.positions); - } - return false; - } - - @Override - public int hashCode() { - // Cast to interpret positions as single argument, not array of the arguments - return Objects.hash((Object) positions); - } - - @Override - public String toString() { - return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions)); - } - - @Override - public SwerveDriveWheelPositions copy() { - return new SwerveDriveWheelPositions(positions); - } - - @Override - public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) { - if (endValue.positions.length != positions.length) { - throw new IllegalArgumentException("Inconsistent number of modules!"); - } - var newPositions = new SwerveModulePosition[positions.length]; - for (int i = 0; i < positions.length; i++) { - newPositions[i] = positions[i].interpolate(endValue.positions[i], t); - } - return new SwerveDriveWheelPositions(newPositions); - } -} diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 342ed2529c..7163cc515b 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -36,7 +36,7 @@ namespace frc { * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. */ -template +template class WPILIB_DLLEXPORT PoseEstimator { public: /** diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc index 43942cfbae..7a0eb71ab5 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc @@ -8,7 +8,7 @@ namespace frc { -template +template PoseEstimator::PoseEstimator( Kinematics& kinematics, Odometry& odometry, @@ -22,7 +22,7 @@ PoseEstimator::PoseEstimator( SetVisionMeasurementStdDevs(visionMeasurementStdDevs); } -template +template void PoseEstimator::SetVisionMeasurementStdDevs( const wpi::array& visionMeasurementStdDevs) { wpi::array r{wpi::empty_array}; @@ -42,7 +42,7 @@ void PoseEstimator::SetVisionMeasurementStdDevs( } } -template +template void PoseEstimator::ResetPosition( const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, const Pose2d& pose) { @@ -51,13 +51,13 @@ void PoseEstimator::ResetPosition( m_poseBuffer.Clear(); } -template +template Pose2d PoseEstimator::GetEstimatedPosition() const { return m_odometry.GetPose(); } -template +template std::optional PoseEstimator::SampleAt( units::second_t timestamp) const { // TODO Replace with std::optional::transform() in C++23 @@ -70,7 +70,7 @@ std::optional PoseEstimator::SampleAt( } } -template +template void PoseEstimator::AddVisionMeasurement( const Pose2d& visionRobotPose, units::second_t timestamp) { // Step 0: If this measurement is old enough to be outside the pose buffer's @@ -128,14 +128,14 @@ void PoseEstimator::AddVisionMeasurement( } } -template +template Pose2d PoseEstimator::Update( const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle, wheelPositions); } -template +template Pose2d PoseEstimator::UpdateWithTime( units::second_t currentTime, const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { @@ -149,7 +149,7 @@ Pose2d PoseEstimator::UpdateWithTime( return GetEstimatedPosition(); } -template +template typename PoseEstimator::InterpolationRecord PoseEstimator::InterpolationRecord::Interpolate( Kinematics& kinematics, @@ -160,8 +160,8 @@ PoseEstimator::InterpolationRecord::Interpolate( return endValue; } else { // Find the new wheel distance measurements. - WheelPositions wheels_lerp = - this->wheelPositions.Interpolate(endValue.wheelPositions, i); + WheelPositions wheels_lerp = kinematics.Interpolate( + this->wheelPositions, endValue.wheelPositions, i); // Find the new gyro angle. auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i); diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 43831d7aff..8aeec4e5b3 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -14,7 +14,6 @@ #include "frc/geometry/Rotation2d.h" #include "frc/kinematics/SwerveDriveKinematics.h" #include "frc/kinematics/SwerveDriveOdometry.h" -#include "frc/kinematics/SwerveDriveWheelPositions.h" #include "units/time.h" namespace frc { @@ -32,8 +31,8 @@ namespace frc { */ template class SwerveDrivePoseEstimator - : public PoseEstimator, - SwerveDriveWheelPositions> { + : public PoseEstimator, + wpi::array> { public: /** * Constructs a SwerveDrivePoseEstimator with default standard deviations @@ -83,69 +82,11 @@ class SwerveDrivePoseEstimator const wpi::array& modulePositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator, - SwerveDriveWheelPositions>( + : PoseEstimator, + wpi::array>( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} - /** - * 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 angle reported by the gyroscope. - * @param modulePositions The current distance and rotation measurements of - * the swerve modules. - * @param pose The position on the field that your robot is at. - */ - void ResetPosition( - const Rotation2d& gyroAngle, - const wpi::array& modulePositions, - const Pose2d& pose) { - PoseEstimator< - SwerveDriveWheelSpeeds, - SwerveDriveWheelPositions>::ResetPosition(gyroAngle, - {modulePositions}, - pose); - } - - /** - * Updates the pose estimator with wheel encoder and gyro information. This - * should be called every loop. - * - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance and rotation measurements of - * the swerve modules. - * @return The estimated robot pose in meters. - */ - Pose2d Update( - const Rotation2d& gyroAngle, - const wpi::array& modulePositions) { - return PoseEstimator< - SwerveDriveWheelSpeeds, - SwerveDriveWheelPositions>::Update(gyroAngle, - {modulePositions}); - } - - /** - * Updates the pose estimator with wheel encoder and gyro information. This - * should be called every loop. - * - * @param currentTime Time at which this method was called, in seconds. - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance traveled and rotations of - * the swerve modules. - * @return The estimated robot pose in meters. - */ - Pose2d UpdateWithTime( - units::second_t currentTime, const Rotation2d& gyroAngle, - const wpi::array& modulePositions) { - return PoseEstimator, - SwerveDriveWheelPositions>:: - UpdateWithTime(currentTime, gyroAngle, {modulePositions}); - } - private: SwerveDriveOdometry m_odometryImpl; }; diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 0f0cff9faa..a1be448c7b 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -88,6 +88,13 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics return ToTwist2d(end.left - start.left, end.right - start.right); } + DifferentialDriveWheelPositions Interpolate( + const DifferentialDriveWheelPositions& start, + const DifferentialDriveWheelPositions& end, double t) const override { + return {wpi::Lerp(start.left, end.left, t), + wpi::Lerp(start.right, end.right, t)}; + } + /// Differential drive trackwidth. units::meter_t trackWidth; }; diff --git a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h index e27cfc69d1..181c9a0abb 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h @@ -20,6 +20,7 @@ namespace frc { * forward kinematics converts wheel speeds into chassis speed. */ template + requires std::copy_constructible class WPILIB_DLLEXPORT Kinematics { public: /** @@ -58,5 +59,18 @@ class WPILIB_DLLEXPORT Kinematics { */ virtual Twist2d ToTwist2d(const WheelPositions& start, const WheelPositions& end) const = 0; + + /** + * Performs interpolation between two values. + * + * @param start The value to start at. + * @param end The value to end at. + * @param t How far between the two values to interpolate. This should be + * bounded to [0, 1]. + * @return The interpolated value. + */ + virtual WheelPositions Interpolate(const WheelPositions& start, + const WheelPositions& end, + double t) const = 0; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index f5a53ca953..6c3d664496 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -166,6 +166,15 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics */ const Translation2d& GetRearRight() const { return m_rearRightWheel; } + MecanumDriveWheelPositions Interpolate( + const MecanumDriveWheelPositions& start, + const MecanumDriveWheelPositions& end, double t) const override { + return {wpi::Lerp(start.frontLeft, end.frontLeft, t), + wpi::Lerp(start.frontRight, end.frontRight, t), + wpi::Lerp(start.rearLeft, end.rearLeft, t), + wpi::Lerp(start.rearRight, end.rearRight, t)}; + } + private: mutable Matrixd<4, 3> m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h index 9e242e2586..53066c7041 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h @@ -24,7 +24,7 @@ namespace frc { * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. */ -template +template class WPILIB_DLLEXPORT Odometry { public: /** diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.inc b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc index 688c1bd293..384a2ddebc 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry.inc +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc @@ -7,7 +7,7 @@ #include "frc/kinematics/Odometry.h" namespace frc { -template +template Odometry::Odometry( const Kinematics& kinematics, const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, @@ -19,7 +19,7 @@ Odometry::Odometry( m_gyroOffset = m_pose.Rotation() - gyroAngle; } -template +template const Pose2d& Odometry::Update( const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { auto angle = gyroAngle + m_gyroOffset; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index f6d4e5df26..da8be0b9b6 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -17,7 +17,6 @@ #include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/Kinematics.h" -#include "frc/kinematics/SwerveDriveWheelPositions.h" #include "frc/kinematics/SwerveModulePosition.h" #include "frc/kinematics/SwerveModuleState.h" #include "units/velocity.h" @@ -25,9 +24,6 @@ namespace frc { -template -using SwerveDriveWheelSpeeds = wpi::array; - /** * Helper class that converts a chassis velocity (dx, dy, and dtheta components) * into individual module states (speed and angle). @@ -52,8 +48,8 @@ using SwerveDriveWheelSpeeds = wpi::array; */ template class SwerveDriveKinematics - : public Kinematics, - SwerveDriveWheelPositions> { + : public Kinematics, + wpi::array> { public: /** * Constructs a swerve drive kinematics object. This takes in a variable @@ -157,7 +153,7 @@ class SwerveDriveKinematics const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation = Translation2d{}) const; - SwerveDriveWheelSpeeds ToWheelSpeeds( + wpi::array ToWheelSpeeds( const ChassisSpeeds& chassisSpeeds) const override { return ToSwerveModuleStates(chassisSpeeds); } @@ -234,14 +230,12 @@ class SwerveDriveKinematics wpi::array moduleDeltas) const; Twist2d ToTwist2d( - const SwerveDriveWheelPositions& start, - const SwerveDriveWheelPositions& end) const override { + const wpi::array& start, + const wpi::array& end) const override { auto result = wpi::array(wpi::empty_array); for (size_t i = 0; i < NumModules; i++) { - auto startModule = start.positions[i]; - auto endModule = end.positions[i]; - result[i] = {endModule.distance - startModule.distance, endModule.angle}; + result[i] = {end[i].distance - start[i].distance, end[i].angle}; } return ToTwist2d(result); } @@ -293,6 +287,18 @@ class SwerveDriveKinematics units::meters_per_second_t attainableMaxRobotTranslationSpeed, units::radians_per_second_t attainableMaxRobotRotationSpeed); + wpi::array Interpolate( + const wpi::array& start, + const wpi::array& end, + double t) const override { + auto result = + wpi::array(wpi::empty_array); + for (size_t i = 0; i < NumModules; ++i) { + result[i] = start[i].Interpolate(end[i], t); + } + return {result}; + } + private: mutable Matrixd m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h index 2e0e553cbd..7c66977bb9 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -13,7 +13,6 @@ #include "Odometry.h" #include "SwerveDriveKinematics.h" -#include "SwerveDriveWheelPositions.h" #include "SwerveModulePosition.h" #include "SwerveModuleState.h" #include "frc/geometry/Pose2d.h" @@ -32,8 +31,8 @@ namespace frc { */ template class SwerveDriveOdometry - : public Odometry, - SwerveDriveWheelPositions> { + : public Odometry, + wpi::array> { public: /** * Constructs a SwerveDriveOdometry object. @@ -48,49 +47,6 @@ class SwerveDriveOdometry const wpi::array& modulePositions, const Pose2d& initialPose = Pose2d{}); - /** - * 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 modulePositions The wheel positions reported by each module. - * @param pose The position on the field that your robot is at. - */ - void ResetPosition( - const Rotation2d& gyroAngle, - const wpi::array& modulePositions, - const Pose2d& pose) { - Odometry< - SwerveDriveWheelSpeeds, - SwerveDriveWheelPositions>::ResetPosition(gyroAngle, - {modulePositions}, - pose); - } - - /** - * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This also takes in an angle parameter - * which is used instead of the angular rate that is calculated from forward - * kinematics. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param modulePositions The current position of all swerve modules. Please - * provide the positions in the same order in which you instantiated your - * SwerveDriveKinematics. - * - * @return The new pose of the robot. - */ - const Pose2d& Update( - const Rotation2d& gyroAngle, - const wpi::array& modulePositions) { - return Odometry< - SwerveDriveWheelSpeeds, - SwerveDriveWheelPositions>::Update(gyroAngle, - {modulePositions}); - } - private: SwerveDriveKinematics m_kinematicsImpl; }; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc index 48ddfec233..2b047c2194 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc @@ -13,9 +13,9 @@ SwerveDriveOdometry::SwerveDriveOdometry( SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, const wpi::array& modulePositions, const Pose2d& initialPose) - : Odometry, - SwerveDriveWheelPositions>( - m_kinematicsImpl, gyroAngle, {modulePositions}, initialPose), + : Odometry, + wpi::array>( + 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/SwerveDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h deleted file mode 100644 index c1686d2efb..0000000000 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h +++ /dev/null @@ -1,51 +0,0 @@ -// 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/kinematics/SwerveModulePosition.h" - -namespace frc { -/** - * Represents the wheel positions for a swerve drive drivetrain. - */ -template -struct WPILIB_DLLEXPORT SwerveDriveWheelPositions { - /** - * The distances driven by the wheels. - */ - wpi::array positions; - - /** - * Checks equality between this SwerveDriveWheelPositions and another object. - * - * @param other The other object. - * @return Whether the two objects are equal. - */ - bool operator==(const SwerveDriveWheelPositions& other) const = default; - - /** - * Checks inequality between this SwerveDriveWheelPositions and another - * object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const SwerveDriveWheelPositions& other) const = default; - - SwerveDriveWheelPositions Interpolate( - const SwerveDriveWheelPositions& endValue, double t) const { - auto result = - wpi::array(wpi::empty_array); - for (size_t i = 0; i < NumModules; i++) { - result[i] = positions[i].Interpolate(endValue.positions[i], t); - } - return {result}; - } -}; -} // namespace frc