diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java index 0cf6d67f4e..1367e51203 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; @@ -50,6 +51,14 @@ class SwerveControllerCommandTest { new SwerveModuleState(0, new Rotation2d(0)) }; + private SwerveModulePosition[] m_modulePositions = + new SwerveModulePosition[] { + new SwerveModulePosition(0, new Rotation2d(0)), + new SwerveModulePosition(0, new Rotation2d(0)), + new SwerveModulePosition(0, new Rotation2d(0)), + new SwerveModulePosition(0, new Rotation2d(0)) + }; + private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI)); @@ -68,7 +77,8 @@ class SwerveControllerCommandTest { new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); private final SwerveDriveOdometry m_odometry = - new SwerveDriveOdometry(m_kinematics, new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0))); + new SwerveDriveOdometry( + m_kinematics, new Rotation2d(0), m_modulePositions, new Pose2d(0, 0, new Rotation2d(0))); @SuppressWarnings("PMD.ArrayIsStoredDirectly") public void setModuleStates(SwerveModuleState[] moduleStates) { @@ -76,7 +86,7 @@ class SwerveControllerCommandTest { } public Pose2d getRobotPose() { - m_odometry.updateWithTime(m_timer.get(), m_angle, m_moduleStates); + m_odometry.update(m_angle, m_modulePositions); return m_odometry.getPoseMeters(); } @@ -111,6 +121,12 @@ class SwerveControllerCommandTest { while (!command.isFinished()) { command.execute(); m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation(); + + for (int i = 0; i < m_modulePositions.length; i++) { + m_modulePositions[i].distanceMeters += m_moduleStates[i].speedMetersPerSecond * 0.005; + m_modulePositions[i].angle = m_moduleStates[i].angle; + } + SimHooks.stepTiming(0.005); } m_timer.stop(); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp index 6891d50171..531e9d2bd6 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp @@ -36,6 +36,10 @@ class SwerveControllerCommandTest : public ::testing::Test { frc::SwerveModuleState{}, frc::SwerveModuleState{}, frc::SwerveModuleState{}, frc::SwerveModuleState{}}; + wpi::array m_modulePositions{ + frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, + frc::SwerveModulePosition{}, frc::SwerveModulePosition{}}; + frc::ProfiledPIDController m_rotController{ 1, 0, 0, frc::TrapezoidProfile::Constraints{ @@ -54,19 +58,15 @@ class SwerveControllerCommandTest : public ::testing::Test { frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2}, frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}}; - frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad, + frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad, m_modulePositions, frc::Pose2d{0_m, 0_m, 0_rad}}; void SetUp() override { frc::sim::PauseTiming(); } void TearDown() override { frc::sim::ResumeTiming(); } - wpi::array getCurrentWheelSpeeds() { - return m_moduleStates; - } - frc::Pose2d getRobotPose() { - m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds()); + m_odometry.Update(m_angle, m_modulePositions); return m_odometry.GetPose(); } }; @@ -95,6 +95,12 @@ TEST_F(SwerveControllerCommandTest, ReachesReference) { while (!command.IsFinished()) { command.Execute(); m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation(); + + for (size_t i = 0; i < m_modulePositions.size(); i++) { + m_modulePositions[i].distance += m_moduleStates[i].speed * 5_ms; + m_modulePositions[i].angle = m_moduleStates[i].angle; + } + frc::sim::StepTiming(5_ms); } m_timer.Stop(); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp index 709f40314e..537da53edc 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp @@ -23,7 +23,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, } void Drivetrain::UpdateOdometry() { - m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(), - m_frontRight.GetState(), m_backLeft.GetState(), - m_backRight.GetState()); + m_odometry.Update(m_gyro.GetRotation2d(), + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_backLeft.GetPosition(), m_backRight.GetPosition()}); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp index 021c73cc14..0f9e27ede1 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp @@ -41,6 +41,11 @@ frc::SwerveModuleState SwerveModule::GetState() const { units::radian_t{m_turningEncoder.GetDistance()}}; } +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return {units::meter_t{m_driveEncoder.GetDistance()}, + units::radian_t{m_turningEncoder.GetDistance()}}; +} + void SwerveModule::SetDesiredState( const frc::SwerveModuleState& referenceState) { // Optimize the reference state to avoid spinning further than 90 degrees diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h index 38fbdbebb5..87233d225d 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h @@ -47,5 +47,9 @@ class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation}; - frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, m_gyro.GetRotation2d()}; + frc::SwerveDriveOdometry<4> m_odometry{ + m_kinematics, + m_gyro.GetRotation2d(), + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_backLeft.GetPosition(), m_backRight.GetPosition()}}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h index 71e54cd895..1861498465 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -23,6 +24,7 @@ class SwerveModule { int driveEncoderChannelA, int driveEncoderChannelB, int turningEncoderChannelA, int turningEncoderChannelB); frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; void SetDesiredState(const frc::SwerveModuleState& state); private: diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp index 6459c6d7b3..9eb82ef880 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -36,13 +36,17 @@ DriveSubsystem::DriveSubsystem() kRearRightDriveEncoderPorts, kRearRightTurningEncoderPorts, kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed}, - m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d{}} {} + m_odometry{kDriveKinematics, + m_gyro.GetRotation2d(), + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, + frc::Pose2d{}} {} void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. - m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(), - m_rearLeft.GetState(), m_frontRight.GetState(), - m_rearRight.GetState()); + m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetPosition(), + m_rearLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearRight.GetPosition()); } void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, @@ -98,5 +102,8 @@ frc::Pose2d DriveSubsystem::GetPose() { } void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { - m_odometry.ResetPosition(pose, units::degree_t{GetHeading()}); + m_odometry.ResetPosition( + pose, units::degree_t{GetHeading()}, + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp index c63187053f..bb0362e012 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp @@ -44,6 +44,11 @@ frc::SwerveModuleState SwerveModule::GetState() { units::radian_t{m_turningEncoder.GetDistance()}}; } +frc::SwerveModulePosition SwerveModule::GetPosition() { + return {units::meter_t{m_driveEncoder.GetDistance()}, + units::radian_t{m_turningEncoder.GetDistance()}}; +} + void SwerveModule::SetDesiredState( const frc::SwerveModuleState& referenceState) { // Optimize the reference state to avoid spinning further than 90 degrees diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h index f0c0a6856e..f0d5edebd2 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -24,6 +25,8 @@ class SwerveModule { frc::SwerveModuleState GetState(); + frc::SwerveModulePosition GetPosition(); + void SetDesiredState(const frc::SwerveModuleState& state); void ResetEncoders(); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp index 33a3233701..892fff3836 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp @@ -27,9 +27,11 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, } void Drivetrain::UpdateOdometry() { - m_poseEstimator.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(), - m_frontRight.GetState(), m_backLeft.GetState(), - m_backRight.GetState()); + m_poseEstimator.Update(m_gyro.GetRotation2d(), + {m_frontLeft.GetState(), m_frontRight.GetState(), + m_backLeft.GetState(), m_backRight.GetState()}, + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_backLeft.GetPosition(), m_backRight.GetPosition()}); // Also apply vision measurements. We use 0.3 seconds in the past as an // example -- on a real robot, this must be calculated based either on latency diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp index c822e9215b..c5a0839650 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp @@ -41,6 +41,11 @@ frc::SwerveModuleState SwerveModule::GetState() const { units::radian_t{m_turningEncoder.GetDistance()}}; } +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return {units::meter_t{m_driveEncoder.GetDistance()}, + units::radian_t{m_turningEncoder.GetDistance()}}; +} + void SwerveModule::SetDesiredState( const frc::SwerveModuleState& referenceState) { // Optimize the reference state to avoid spinning further than 90 degrees diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h index 3566657b90..c9015c6905 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h @@ -50,6 +50,12 @@ class Drivetrain { // Gains are for example purposes only - must be determined for your own // robot! frc::SwerveDrivePoseEstimator<4> m_poseEstimator{ - frc::Rotation2d{}, frc::Pose2d{}, m_kinematics, - {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; + frc::Rotation2d{}, + frc::Pose2d{}, + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_backLeft.GetPosition(), m_backRight.GetPosition()}, + m_kinematics, + {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, + {0.05, 0.05, 0.05, 0.05, 0.05}, + {0.1, 0.1, 0.1}}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h index 5242de26c1..049d50731e 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -23,6 +24,7 @@ class SwerveModule { int driveEncoderChannelA, int driveEncoderChannelB, int turningEncoderChannelA, int turningEncoderChannelB); frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; void SetDesiredState(const frc::SwerveModuleState& state); private: diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 0df6096cba..03929d5269 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -63,9 +63,9 @@ public class Drivetrain { public void updateOdometry() { m_odometry.update( m_gyro.getRotation2d(), - m_frontLeft.getState(), - m_frontRight.getState(), - m_backLeft.getState(), - m_backRight.getState()); + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_backLeft.getPosition(), + m_backRight.getPosition()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index 9749c2df8e..9acc3b04c3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Encoder; @@ -91,6 +92,16 @@ public class SwerveModule { return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get())); } + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.get())); + } + /** * Sets the desired state for the module. * diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 0ec4d7ce8a..dc0a17d755 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -67,10 +67,10 @@ public class DriveSubsystem extends SubsystemBase { // Update the odometry in the periodic block m_odometry.update( m_gyro.getRotation2d(), - m_frontLeft.getState(), - m_frontRight.getState(), - m_rearLeft.getState(), - m_rearRight.getState()); + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition()); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java index 494d0ab20a..53f6593178 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Encoder; @@ -87,6 +88,16 @@ public class SwerveModule { return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get())); } + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.get())); + } + /** * Sets the desired state for the module. * diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index c6231c210a..b6b03b9e5a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -4,12 +4,17 @@ package edu.wpi.first.wpilibj.examples.swervedriveposeestimator; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.math.numbers.N7; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Timer; @@ -37,13 +42,22 @@ public class Drivetrain { /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used below are robot specific, and should be tuned. */ - private final SwerveDrivePoseEstimator m_poseEstimator = - new SwerveDrivePoseEstimator( + private final SwerveDrivePoseEstimator m_poseEstimator = + new SwerveDrivePoseEstimator( + Nat.N7(), + Nat.N7(), + Nat.N5(), m_gyro.getRotation2d(), new Pose2d(), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_backLeft.getPosition(), + m_backRight.getPosition() + }, m_kinematics, - VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), - VecBuilder.fill(Units.degreesToRadians(0.01)), + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05, 0.05, 0.05), + VecBuilder.fill(Units.degreesToRadians(0.01), 0.01, 0.01, 0.01, 0.01), VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); public Drivetrain() { @@ -75,10 +89,18 @@ public class Drivetrain { public void updateOdometry() { m_poseEstimator.update( m_gyro.getRotation2d(), - m_frontLeft.getState(), - m_frontRight.getState(), - m_backLeft.getState(), - m_backRight.getState()); + new SwerveModuleState[] { + m_frontLeft.getState(), + m_frontRight.getState(), + m_backLeft.getState(), + m_backRight.getState() + }, + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_backLeft.getPosition(), + m_backRight.getPosition() + }); // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on // a real robot, this must be calculated based either on latency or timestamps. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java index d7c43dd052..8de7292610 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Encoder; @@ -91,6 +92,16 @@ public class SwerveModule { return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get())); } + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.get())); + } + /** * Sets the desired state for the module. * 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 ce502a9275..09cd6ec21b 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 @@ -4,16 +4,16 @@ package edu.wpi.first.math.estimator; -import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; import edu.wpi.first.math.StateSpaceUtil; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -26,10 +26,16 @@ import java.util.function.BiConsumer; * correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate * drop-in for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}. * + *

The generic arguments to this class define the size of the state, input and output vectors + * used in the underlying {@link UnscentedKalmanFilter Unscented Kalman Filter}. {@link Num States} + * must be equal to the module count + 3. {@link Num Inputs} must be equal to the module count + 3. + * {@link Num Outputs} must be equal to the module count + 1. + * *

{@link SwerveDrivePoseEstimator#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 using - * the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d, - * Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}. + * the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Nat, Nat, + * Nat, Rotation2d, Pose2d, SwerveModulePosition[], SwerveDriveKinematics, Matrix, Matrix, Matrix, + * double)}. * *

{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you * want; if you never call it, then this class will behave mostly like regular encoder odometry. @@ -37,21 +43,25 @@ import java.util.function.BiConsumer; *

The state-space system used internally has the following states (x), inputs (u), and outputs * (y): * - *

x = [x, y, theta]ᵀ in the field coordinate system containing x position, y - * position, and heading. + *

x = [x, y, theta, s_0, ... s_n]ᵀ in the field coordinate system containing + * x position, y position, and heading, followed by the distance travelled by each wheel. * - *

u = [v_x, v_y, omega]ᵀ containing x velocity, y velocity, and angular rate - * in the field coordinate system. + *

u = [v_x, v_y, omega, v_0, ... v_n]ᵀ containing x velocity, y velocity, and + * angular rate in the field coordinate system, followed by the velocity measured at each wheel. * *

y = [x, y, theta]ᵀ from vision containing x position, y position, and * heading; or y = [theta]ᵀ containing gyro heading. */ -public class SwerveDrivePoseEstimator { - private final UnscentedKalmanFilter m_observer; +public class SwerveDrivePoseEstimator { + private final UnscentedKalmanFilter m_observer; private final SwerveDriveKinematics m_kinematics; - private final BiConsumer, Matrix> m_visionCorrect; + private final BiConsumer, Matrix> m_visionCorrect; private final TimeInterpolatableBuffer m_poseBuffer; + private final Nat m_states; + private final Nat m_inputs; + private final Nat m_outputs; + private final double m_nominalDt; // Seconds private double m_prevTimeSeconds = -1.0; @@ -63,29 +73,41 @@ public class SwerveDrivePoseEstimator { /** * Constructs a SwerveDrivePoseEstimator. * + * @param states The size of the state vector. + * @param inputs The size of the input vector. + * @param outputs The size of the outputs vector. * @param gyroAngle The current gyro angle. * @param initialPoseMeters The starting pose estimate. + * @param modulePositions The current distance measurements and rotations of the swerve modules. * @param kinematics A correctly-configured kinematics object for your drivetrain. * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your - * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in - * meters and radians. - * @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number - * to trust sensor readings from the gyro less. This matrix is in the form [theta], with units - * in radians. + * model's state estimates less. This matrix is in the form [x, y, theta, s_0, ... s_n]ᵀ, with + * units in meters and radians, then meters. + * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements. + * Increase these numbers to trust sensor readings from encoders and gyros less. This matrix + * is in the form [theta, s_0, ... s_n], with units in radians followed by meters. * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these * numbers to trust global measurements from vision less. This matrix is in the form [x, y, * theta]ᵀ, with units in meters and radians. */ public SwerveDrivePoseEstimator( + Nat states, + Nat inputs, + Nat outputs, Rotation2d gyroAngle, Pose2d initialPoseMeters, + SwerveModulePosition[] modulePositions, SwerveDriveKinematics kinematics, - Matrix stateStdDevs, - Matrix localMeasurementStdDevs, + Matrix stateStdDevs, + Matrix localMeasurementStdDevs, Matrix visionMeasurementStdDevs) { this( + states, + inputs, + outputs, gyroAngle, initialPoseMeters, + modulePositions, kinematics, stateStdDevs, localMeasurementStdDevs, @@ -96,36 +118,72 @@ public class SwerveDrivePoseEstimator { /** * Constructs a SwerveDrivePoseEstimator. * + * @param states The size of the state vector. + * @param inputs The size of the input vector. + * @param outputs The size of the outputs vector. * @param gyroAngle The current gyro angle. * @param initialPoseMeters The starting pose estimate. + * @param modulePositions The current distance measurements and rotations of the swerve modules. * @param kinematics A correctly-configured kinematics object for your drivetrain. * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your - * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in - * meters and radians. + * model's state estimates less. This matrix is in the form [x, y, theta, s_0, ... s_n]ᵀ, with + * units in meters and radians, then meters. * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements. * Increase these numbers to trust sensor readings from encoders and gyros less. This matrix - * is in the form [theta], with units in radians. + * is in the form [theta, s_0, ... s_n], with units in radians followed by meters. * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these * numbers to trust global measurements from vision less. This matrix is in the form [x, y, * theta]ᵀ, with units in meters and radians. * @param nominalDtSeconds The time in seconds between each robot loop. */ public SwerveDrivePoseEstimator( + Nat states, + Nat inputs, + Nat outputs, Rotation2d gyroAngle, Pose2d initialPoseMeters, + SwerveModulePosition[] modulePositions, SwerveDriveKinematics kinematics, - Matrix stateStdDevs, - Matrix localMeasurementStdDevs, + Matrix stateStdDevs, + Matrix localMeasurementStdDevs, Matrix visionMeasurementStdDevs, double nominalDtSeconds) { + this.m_states = states; + this.m_inputs = inputs; + this.m_outputs = outputs; + + if (states.getNum() != modulePositions.length + 3) { + throw new IllegalArgumentException( + String.format( + "Number of states (%s) must be 3 + " + + "the number of modules provided in constructor (%s).", + states.getNum(), modulePositions.length)); + } + + if (inputs.getNum() != modulePositions.length + 3) { + throw new IllegalArgumentException( + String.format( + "Number of inputs (%s) must be 3 + " + + "the number of modules provided in constructor (%s).", + inputs.getNum(), modulePositions.length)); + } + + if (outputs.getNum() != modulePositions.length + 1) { + throw new IllegalArgumentException( + String.format( + "Number of outputs (%s) must be 3 + " + + "the number of modules provided in constructor (%s).", + outputs.getNum(), modulePositions.length)); + } + m_nominalDt = nominalDtSeconds; m_observer = new UnscentedKalmanFilter<>( - Nat.N3(), - Nat.N1(), - (x, u) -> u, - (x, u) -> x.extractRowVector(2), + states, + outputs, + (x, u) -> u.block(states.getNum(), 1, 0, 0), + (x, u) -> x.block(states.getNum() - 2, 1, 2, 0), stateStdDevs, localMeasurementStdDevs, AngleStatistics.angleMean(2), @@ -146,7 +204,7 @@ public class SwerveDrivePoseEstimator { Nat.N3(), u, y, - (x, u1) -> x, + (x, u1) -> x.block(3, 1, 0, 0), m_visionContR, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), @@ -155,7 +213,18 @@ public class SwerveDrivePoseEstimator { m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle); m_previousAngle = initialPoseMeters.getRotation(); - m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters)); + + var poseVec = StateSpaceUtil.poseTo3dVector(initialPoseMeters); + Matrix xhat = new Matrix(states, Nat.N1()); + xhat.set(0, 0, poseVec.get(0, 0)); + xhat.set(1, 0, poseVec.get(1, 0)); + xhat.set(2, 0, poseVec.get(2, 0)); + + for (int index = 3; index < states.getNum(); index++) { + xhat.set(index, 0, modulePositions[index - 3].distanceMeters); + } + + m_observer.setXhat(xhat); } /** @@ -179,13 +248,25 @@ public class SwerveDrivePoseEstimator { * * @param poseMeters The position on the field that your robot is at. * @param gyroAngle The angle reported by the gyroscope. + * @param modulePositions The current distance measurements and rotations of the swerve modules. */ - public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { + public void resetPosition( + Pose2d poseMeters, Rotation2d gyroAngle, SwerveModulePosition... modulePositions) { // Reset state estimate and error covariance m_observer.reset(); m_poseBuffer.clear(); - m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters)); + var poseVec = StateSpaceUtil.poseTo3dVector(poseMeters); + Matrix xhat = new Matrix(m_states, Nat.N1()); + xhat.set(0, 0, poseVec.get(0, 0)); + xhat.set(1, 0, poseVec.get(1, 0)); + xhat.set(2, 0, poseVec.get(2, 0)); + + for (int index = 3; index < m_states.getNum(); index++) { + xhat.set(index, 0, modulePositions[index - 3].distanceMeters); + } + + m_observer.setXhat(xhat); m_prevTimeSeconds = -1; @@ -225,7 +306,7 @@ public class SwerveDrivePoseEstimator { var sample = m_poseBuffer.getSample(timestampSeconds); if (sample.isPresent()) { m_visionCorrect.accept( - new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0), + new Matrix(m_inputs, Nat.N1()), StateSpaceUtil.poseTo3dVector( getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get())))); } @@ -271,10 +352,14 @@ public class SwerveDrivePoseEstimator { * * @param gyroAngle The current gyro angle. * @param moduleStates The current velocities and rotations of the swerve modules. + * @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, SwerveModuleState... moduleStates) { - return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates); + public Pose2d update( + Rotation2d gyroAngle, + SwerveModuleState[] moduleStates, + SwerveModulePosition[] modulePositions) { + return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates, modulePositions); } /** @@ -285,10 +370,14 @@ public class SwerveDrivePoseEstimator { * @param currentTimeSeconds Time at which this method was called, in seconds. * @param gyroAngle The current gyroscope angle. * @param moduleStates The current velocities and rotations of the swerve modules. + * @param 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, SwerveModuleState... moduleStates) { + double currentTimeSeconds, + Rotation2d gyroAngle, + SwerveModuleState[] moduleStates, + SwerveModulePosition[] modulePositions) { double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt; m_prevTimeSeconds = currentTimeSeconds; @@ -300,10 +389,23 @@ public class SwerveDrivePoseEstimator { new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) .rotateBy(angle); - var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega); + var u = new Matrix(m_inputs, Nat.N1()); + + u.set(0, 0, fieldRelativeVelocities.getX()); + u.set(1, 0, fieldRelativeVelocities.getY()); + u.set(2, 0, omega); + for (int index = 3; index < m_inputs.getNum(); index++) { + u.set(index, 0, moduleStates[index - 3].speedMetersPerSecond); + } + m_previousAngle = angle; - var localY = VecBuilder.fill(angle.getRadians()); + var localY = new Matrix(m_outputs, Nat.N1()); + localY.set(0, 0, angle.getRadians()); + for (int index = 1; index < m_outputs.getNum(); index++) { + localY.set(index, 0, modulePositions[index - 1].distanceMeters); + } + m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition()); m_observer.predict(u, dt); m_observer.correct(u, localY); 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 3a5349ceb3..cb3f460a62 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 @@ -8,6 +8,7 @@ import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import java.util.Arrays; import java.util.Collections; import org.ejml.simple.SimpleMatrix; @@ -185,6 +186,35 @@ public class SwerveDriveKinematics { chassisSpeedsVector.get(2, 0)); } + /** + * 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 + * data from the real-world speed and angle of each module on the robot. + * + * @param wheelDeltas The latest change in position of the modules (as a SwerveModulePosition + * type) as measured from respective encoders and gyros. The order of the swerve module states + * should be same as passed into the constructor of this class. + * @return The resulting Twist2d. + */ + public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas) { + if (wheelDeltas.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1); + + for (int i = 0; i < m_numModules; i++) { + var module = wheelDeltas[i]; + moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos()); + moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin()); + } + + var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix); + return new Twist2d( + chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0)); + } + /** * Renormalizes the wheel speeds if any individual speed is above the specified maximum. * 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 8b4161e3f0..73369f254d 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 @@ -8,8 +8,6 @@ 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.Rotation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.util.WPIUtilJNI; /** * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field @@ -22,24 +20,38 @@ import edu.wpi.first.util.WPIUtilJNI; public class SwerveDriveOdometry { private final SwerveDriveKinematics m_kinematics; private Pose2d m_poseMeters; - private double m_prevTimeSeconds = -1; private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; + private final int m_numModules; + private SwerveModulePosition[] m_previousModulePositions; /** * Constructs a SwerveDriveOdometry 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 SwerveDriveOdometry( - SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) { + SwerveDriveKinematics kinematics, + Rotation2d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose2d initialPose) { m_kinematics = kinematics; m_poseMeters = initialPose; m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_previousAngle = initialPose.getRotation(); + m_numModules = modulePositions.length; + + m_previousModulePositions = new SwerveModulePosition[m_numModules]; + for (int index = 0; index < m_numModules; index++) { + m_previousModulePositions[index] = + new SwerveModulePosition( + modulePositions[index].distanceMeters, modulePositions[index].angle); + } + MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1); } @@ -48,9 +60,13 @@ public class SwerveDriveOdometry { * * @param kinematics The swerve drive kinematics for your drivetrain. * @param gyroAngle The angle reported by the gyroscope. + * @param modulePositions The wheel positions reported by each module. */ - public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) { - this(kinematics, gyroAngle, new Pose2d()); + public SwerveDriveOdometry( + SwerveDriveKinematics kinematics, + Rotation2d gyroAngle, + SwerveModulePosition... modulePositions) { + this(kinematics, gyroAngle, modulePositions, new Pose2d()); } /** @@ -61,11 +77,24 @@ public class SwerveDriveOdometry { * * @param pose The position on the field that your robot is at. * @param gyroAngle The angle reported by the gyroscope. + * @param modulePositions The wheel positions reported by each module. */ - public void resetPosition(Pose2d pose, Rotation2d gyroAngle) { + public void resetPosition( + Pose2d pose, 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"); + } + m_poseMeters = pose; m_previousAngle = pose.getRotation(); m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + for (int index = 0; index < m_numModules; index++) { + m_previousModulePositions[index] = + new SwerveModulePosition( + modulePositions[index].distanceMeters, modulePositions[index].angle); + } } /** @@ -77,40 +106,6 @@ public class SwerveDriveOdometry { 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 the current time as a parameter to calculate period (difference - * between two timestamps). The period is used to calculate the change in distance from a - * velocity. This also takes in an angle parameter which is used instead of the angular rate that - * is calculated from forward kinematics. - * - * @param currentTimeSeconds The current time in seconds. - * @param gyroAngle The angle reported by the gyroscope. - * @param moduleStates The current state of all swerve modules. Please provide the states in the - * same order in which you instantiated your SwerveDriveKinematics. - * @return The new pose of the robot. - */ - public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) { - double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; - m_prevTimeSeconds = currentTimeSeconds; - - var angle = gyroAngle.plus(m_gyroOffset); - - var chassisState = m_kinematics.toChassisSpeeds(moduleStates); - var newPose = - m_poseMeters.exp( - new Twist2d( - chassisState.vxMetersPerSecond * period, - chassisState.vyMetersPerSecond * period, - angle.minus(m_previousAngle).getRadians())); - - m_previousAngle = angle; - m_poseMeters = new Pose2d(newPose.getTranslation(), angle); - - return m_poseMeters; - } - /** * 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 @@ -119,11 +114,40 @@ public class SwerveDriveOdometry { * rate that is calculated from forward kinematics. * * @param gyroAngle The angle reported by the gyroscope. - * @param moduleStates The current state of all swerve modules. Please provide the states in the - * same order in which you instantiated your SwerveDriveKinematics. + * @param modulePositions The current position of all swerve modules. Please provide the positions + * in the same order in which you instantiated your SwerveDriveKinematics. * @return The new pose of the robot. */ - public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) { - return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates); + 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"); + } + + var moduleDeltas = new SwerveModulePosition[m_numModules]; + for (int index = 0; index < m_numModules; index++) { + var current = modulePositions[index]; + var previous = m_previousModulePositions[index]; + + moduleDeltas[index] = + new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle); + } + + var angle = gyroAngle.plus(m_gyroOffset); + + var twist = m_kinematics.toTwist2d(moduleDeltas); + twist.dtheta = angle.minus(m_previousAngle).getRadians(); + + var newPose = m_poseMeters.exp(twist); + + m_previousAngle = angle; + m_poseMeters = new Pose2d(newPose.getTranslation(), angle); + for (int index = 0; index < m_numModules; index++) { + m_previousModulePositions[index] = + new SwerveModulePosition( + modulePositions[index].distanceMeters, modulePositions[index].angle); + } + return m_poseMeters; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java new file mode 100644 index 0000000000..43b787d404 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java @@ -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. + +package edu.wpi.first.math.kinematics; + +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Objects; + +/** Represents the state of one swerve module. */ +public class SwerveModulePosition implements Comparable { + /** Distance measured by the wheel of the module. */ + public double distanceMeters; + + /** Angle of the module. */ + public Rotation2d angle = Rotation2d.fromDegrees(0); + + /** Constructs a SwerveModulePosition with zeros for speed and angle. */ + public SwerveModulePosition() {} + + /** + * Constructs a SwerveModulePosition. + * + * @param distanceMeters The distance measured by the wheel of the module. + * @param angle The angle of the module. + */ + public SwerveModulePosition(double distanceMeters, Rotation2d angle) { + this.distanceMeters = distanceMeters; + this.angle = angle; + } + + @Override + public boolean equals(Object obj) { + if (obj instanceof SwerveModulePosition) { + return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(distanceMeters); + } + + /** + * Compares two swerve module positions. One swerve module is "greater" than the other if its + * speed is higher than the other. + * + * @param other The other swerve module. + * @return 1 if this is greater, 0 if both are equal, -1 if other is greater. + */ + @Override + public int compareTo(SwerveModulePosition other) { + return Double.compare(this.distanceMeters, other.distanceMeters); + } + + @Override + public String toString() { + return String.format( + "SwerveModulePosition(Distance: %.2f m/s, Angle: %s)", distanceMeters, angle); + } +} diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index ff5103645b..2df255d007 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -38,11 +38,13 @@ namespace frc { * The state-space system used internally has the following states (x), inputs * (u), and outputs (y): * - * x = [x, y, theta]ᵀ in the field coordinate system - * containing x position, y position, and heading. + * x = [x, y, theta, s_0, ... s_n]ᵀ in the field coordinate + * system containing x position, y position, and heading, followed by the + * distance travelled by each wheel. * - * u = [v_x, v_y, omega]ᵀ containing x velocity, y velocity, - * and angular velocity in the field coordinate system. + * u = [v_x, v_y, omega, v_0, ... v_n]ᵀ containing x + * velocity, y velocity, and angular velocity in the field coordinate system, + * followed by the velocity measured at each wheel. * * y = [x, y, theta]ᵀ from vision containing x position, y * position, and heading; or y = [theta]ᵀ containing gyro @@ -51,22 +53,29 @@ namespace frc { template class SwerveDrivePoseEstimator { public: + static constexpr size_t States = 3 + NumModules; + static constexpr size_t Inputs = 3 + NumModules; + static constexpr size_t Outputs = 1 + NumModules; + /** * Constructs a SwerveDrivePoseEstimator. * * @param gyroAngle The current gyro angle. * @param initialPose The starting pose estimate. + * @param modulePositions The current distance and rotation + * measurements of the swerve modules. * @param kinematics A correctly-configured kinematics object * for your drivetrain. * @param stateStdDevs Standard deviations of model states. * Increase these numbers to trust your * model's state estimates less. This matrix - * is in the form [x, y, theta]ᵀ, with units - * in meters and radians. + * is in the form [x, y, theta, s_0, ... + * s_n]ᵀ, with units in meters and radians, then meters. * @param localMeasurementStdDevs Standard deviation of the gyro measurement. * Increase this number to trust sensor * readings from the gyro less. This matrix is - * in the form [theta], with units in radians. + * in the form [theta, s_0, ... s_n], with + * units in radians followed by meters. * @param visionMeasurementStdDevs Standard deviations of the vision * measurements. Increase these numbers to * trust global measurements from vision @@ -78,33 +87,48 @@ class SwerveDrivePoseEstimator { */ SwerveDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, + const wpi::array modulePositions, SwerveDriveKinematics& kinematics, - const wpi::array& stateStdDevs, - const wpi::array& localMeasurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& localMeasurementStdDevs, const wpi::array& visionMeasurementStdDevs, units::second_t nominalDt = 20_ms) - : m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; }, - [](const Vectord<3>& x, const Vectord<3>& u) { - return x.block<1, 1>(2, 0); + : m_observer([](const Vectord& x, + const Vectord& u) { return u; }, + [](const Vectord& x, const Vectord& u) { + return x.template block(2, 0); }, stateStdDevs, localMeasurementStdDevs, - frc::AngleMean<3, 3>(2), frc::AngleMean<1, 3>(0), - frc::AngleResidual<3>(2), frc::AngleResidual<1>(0), - frc::AngleAdd<3>(2), nominalDt), + frc::AngleMean(2), + frc::AngleMean(0), + frc::AngleResidual(2), + frc::AngleResidual(0), frc::AngleAdd(2), + nominalDt), m_kinematics(kinematics), m_nominalDt(nominalDt) { SetVisionMeasurementStdDevs(visionMeasurementStdDevs); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) { - m_observer.Correct<3>( - u, y, [](const Vectord<3>& x, const Vectord<3>& u) { return x; }, - m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), - frc::AngleResidual<3>(2), frc::AngleAdd<3>(2)); + m_visionCorrect = [&](const Vectord& u, const Vectord<3>& y) { + m_observer.template Correct<3>( + u, y, + [](const Vectord& x, const Vectord& u) { + return x.template block<3, 1>(0, 0); + }, + m_visionContR, frc::AngleMean<3, States>(2), frc::AngleResidual<3>(2), + frc::AngleResidual(2), frc::AngleAdd(2)); }; // Set initial state. - m_observer.SetXhat(PoseTo3dVector(initialPose)); + Vectord xhat; + auto poseVec = PoseTo3dVector(initialPose); + xhat(0) = poseVec(0); + xhat(1) = poseVec(1); + xhat(2) = poseVec(2); + for (size_t i = 0; i < NumModules; i++) { + xhat(3 + i) = modulePositions[i].distance.value(); + } + m_observer.SetXhat(xhat); // Calculate offsets. m_gyroOffset = initialPose.Rotation() - gyroAngle; @@ -117,15 +141,27 @@ class SwerveDrivePoseEstimator { * The gyroscope angle does not need to be reset in the user's robot code. * The library automatically takes care of offsetting the gyro angle. * - * @param pose The position on the field that your robot is at. - * @param gyroAngle The angle reported by the gyroscope. + * @param pose The position on the field that your robot is at. + * @param gyroAngle The angle reported by the gyroscope. + * @param modulePositions The current distance and rotation measurements of + * the swerve modules. */ - void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) { + void ResetPosition( + const Pose2d& pose, const Rotation2d& gyroAngle, + wpi::array modulePositions) { // Reset state estimate and error covariance m_observer.Reset(); m_poseBuffer.Clear(); - m_observer.SetXhat(PoseTo3dVector(pose)); + Vectord xhat; + auto poseVec = PoseTo3dVector(pose); + xhat(0) = poseVec(0); + xhat(1) = poseVec(1); + xhat(2) = poseVec(2); + for (size_t i = 0; i < NumModules; i++) { + xhat(3 + i) = modulePositions[i].distance.value(); + } + m_observer.SetXhat(xhat); m_prevTime = -1_s; @@ -187,7 +223,7 @@ class SwerveDrivePoseEstimator { void AddVisionMeasurement(const Pose2d& visionRobotPose, units::second_t timestamp) { if (auto sample = m_poseBuffer.Sample(timestamp)) { - m_visionCorrect(Vectord<3>::Zero(), + m_visionCorrect(Vectord::Zero(), PoseTo3dVector(GetEstimatedPosition().TransformBy( visionRobotPose - sample.value()))); } @@ -240,15 +276,19 @@ class SwerveDrivePoseEstimator { * information. This should be called every loop, and the correct loop period * must be passed into the constructor of this class. * - * @param gyroAngle The current gyro angle. - * @param moduleStates The current velocities and rotations of the swerve - * modules. + * @param gyroAngle The current gyro angle. + * @param moduleStates The current velocities and rotations of the swerve + * modules. + * @param modulePositions The current distance and rotation measurements of + * the swerve modules. * @return The estimated pose of the robot in meters. */ - template - Pose2d Update(const Rotation2d& gyroAngle, ModuleState&&... moduleStates) { + Pose2d Update( + const Rotation2d& gyroAngle, + const wpi::array moduleStates, + const wpi::array modulePositions) { return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle, - moduleStates...); + moduleStates, modulePositions); } /** @@ -256,31 +296,43 @@ class SwerveDrivePoseEstimator { * information. This should be called every loop, and the correct loop period * must be passed into the constructor of this class. * - * @param currentTime Time at which this method was called, in seconds. - * @param gyroAngle The current gyroscope angle. - * @param moduleStates The current velocities and rotations of the swerve - * modules. + * @param currentTime Time at which this method was called, in seconds. + * @param gyroAngle The current gyro angle. + * @param moduleStates The current velocities and rotations of the swerve + * modules. + * @param modulePositions The current distance travelled and rotations of + * the swerve modules. * @return The estimated pose of the robot in meters. */ - template - Pose2d UpdateWithTime(units::second_t currentTime, - const Rotation2d& gyroAngle, - ModuleState&&... moduleStates) { + Pose2d UpdateWithTime( + units::second_t currentTime, const Rotation2d& gyroAngle, + const wpi::array moduleStates, + const wpi::array modulePositions) { auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt; m_prevTime = currentTime; auto angle = gyroAngle + m_gyroOffset; auto omega = (angle - m_previousAngle).Radians() / dt; - auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates...); + auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates); auto fieldRelativeSpeeds = Translation2d{chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s}.RotateBy( angle); - Vectord<3> u{fieldRelativeSpeeds.X().value(), - fieldRelativeSpeeds.Y().value(), omega.value()}; + Vectord u; + u(0) = fieldRelativeSpeeds.X().value(); + u(1) = fieldRelativeSpeeds.Y().value(); + u(2) = omega.value(); + for (size_t i = 0; i < NumModules; i++) { + u(3 + i) = moduleStates[i].speed.value(); + } + + Vectord localY; + localY(0) = angle.Radians().value(); + for (size_t i = 0; i < NumModules; i++) { + localY(1 + i) = modulePositions[i].distance.value(); + } - Vectord<1> localY{angle.Radians().value()}; m_previousAngle = angle; m_poseBuffer.AddSample(currentTime, GetEstimatedPosition()); @@ -292,10 +344,11 @@ class SwerveDrivePoseEstimator { } private: - UnscentedKalmanFilter<3, 3, 1> m_observer; + UnscentedKalmanFilter m_observer; SwerveDriveKinematics& m_kinematics; TimeInterpolatableBuffer m_poseBuffer{1.5_s}; - std::function& u, const Vectord<3>& y)> m_visionCorrect; + std::function& u, const Vectord<3>& y)> + m_visionCorrect; Eigen::Matrix3d m_visionContR; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 3ac453f716..6586cbc6cf 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -13,7 +13,9 @@ #include "frc/EigenCore.h" #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" +#include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/SwerveModulePosition.h" #include "frc/kinematics/SwerveModuleState.h" #include "units/velocity.h" #include "wpimath/MathShared.h" @@ -162,6 +164,38 @@ class SwerveDriveKinematics { ChassisSpeeds ToChassisSpeeds( wpi::array moduleStates) const; + /** + * Performs forward kinematics to return the resulting Twist2d from the + * given module position deltas. This method is often used for odometry -- + * determining the robot's position on the field using data from the + * real-world position delta and angle of each module on the robot. + * + * @param wheelDeltas The latest change in position of the modules (as a + * SwerveModulePosition type) as measured from respective encoders and gyros. + * The order of the swerve module states should be same as passed into the + * constructor of this class. + * + * @return The resulting Twist2d. + */ + template + Twist2d ToTwist2d(ModuleDeltas&&... wheelDeltas) const; + + /** + * Performs forward kinematics to return the resulting Twist2d from the + * given module position deltas. This method is often used for odometry -- + * determining the robot's position on the field using data from the + * real-world position delta and angle of each module on the robot. + * + * @param wheelDeltas The latest change in position of the modules (as a + * SwerveModulePosition type) as measured from respective encoders and gyros. + * The order of the swerve module states should be same as passed into the + * constructor of this class. + * + * @return The resulting Twist2d. + */ + Twist2d ToTwist2d( + wpi::array wheelDeltas) const; + /** * Renormalizes the wheel speeds if any individual speed is above the * specified maximum. diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index 8e4b381fec..dc3f14b710 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -95,6 +95,39 @@ ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( units::radians_per_second_t{chassisSpeedsVector(2)}}; } +template +template +Twist2d SwerveDriveKinematics::ToTwist2d( + ModuleDeltas&&... wheelDeltas) const { + static_assert(sizeof...(wheelDeltas) == NumModules, + "Number of modules is not consistent with number of wheel " + "locations provided in constructor."); + + wpi::array moduleDeltas{wheelDeltas...}; + + return this->ToTwist2d(moduleDeltas); +} + +template +Twist2d SwerveDriveKinematics::ToTwist2d( + wpi::array moduleDeltas) const { + Matrixd moduleDeltaMatrix; + + for (size_t i = 0; i < NumModules; ++i) { + SwerveModulePosition module = moduleDeltas[i]; + moduleDeltaMatrix(i * 2, 0) = module.distance.value() * module.angle.Cos(); + moduleDeltaMatrix(i * 2 + 1, 0) = + module.distance.value() * module.angle.Sin(); + } + + Eigen::Vector3d chassisDeltaVector = + m_forwardKinematics.solve(moduleDeltaMatrix); + + return {units::meter_t{chassisDeltaVector(0)}, + units::meter_t{chassisDeltaVector(1)}, + units::radian_t{chassisDeltaVector(2)}}; +} + template void SwerveDriveKinematics::DesaturateWheelSpeeds( wpi::array* moduleStates, diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h index 42efd49b86..1c5fab04bc 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -12,7 +12,7 @@ #include #include "SwerveDriveKinematics.h" -#include "SwerveModuleState.h" +#include "SwerveModulePosition.h" #include "frc/geometry/Pose2d.h" #include "units/time.h" @@ -35,11 +35,13 @@ class SwerveDriveOdometry { * * @param kinematics The swerve drive kinematics for your drivetrain. * @param gyroAngle The angle reported by the gyroscope. + * @param modulePositions The wheel positions reported by each module. * @param initialPose The starting position of the robot on the field. */ - SwerveDriveOdometry(SwerveDriveKinematics kinematics, - const Rotation2d& gyroAngle, - const Pose2d& initialPose = Pose2d{}); + SwerveDriveOdometry( + SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, + const wpi::array modulePositions, + const Pose2d& initialPose = Pose2d{}); /** * Resets the robot's position on the field. @@ -49,12 +51,25 @@ class SwerveDriveOdometry { * * @param pose The position on the field that your robot is at. * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The wheel positions reported by each module. */ - void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) { - m_pose = pose; - m_previousAngle = pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; - } + template + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle, + ModulePositions&&... wheelPositions); + + /** + * Resets the robot's position on the field. + * + * The gyroscope angle does not need to be reset here on the user's robot + * code. The library automatically takes care of offsetting the gyro angle. + * + * @param pose The position on the field that your robot is at. + * @param gyroAngle The angle reported by the gyroscope. + * @param modulePositions The wheel positions reported by each module. + */ + void ResetPosition( + const Pose2d& pose, const Rotation2d& gyroAngle, + const wpi::array modulePositions); /** * Returns the position of the robot on the field. @@ -70,47 +85,44 @@ class SwerveDriveOdometry { * also takes in an angle parameter which is used instead of the * angular rate that is calculated from forward kinematics. * - * @param currentTime The current time. * @param gyroAngle The angle reported by the gyroscope. - * @param moduleStates The current state of all swerve modules. Please provide - * the states in the same order in which you instantiated - * your SwerveDriveKinematics. + * @param wheelPositions The current position of all swerve modules. Please + * provide the positions in the same order in which you instantiated your + * SwerveDriveKinematics. * * @return The new pose of the robot. */ - template - const Pose2d& UpdateWithTime(units::second_t currentTime, - const Rotation2d& gyroAngle, - ModuleStates&&... moduleStates); + template + const Pose2d& Update(const Rotation2d& gyroAngle, + ModulePositions&&... wheelPositions); /** * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This method automatically calculates - * the current time to calculate period (difference between two timestamps). - * The period is used to calculate the change in distance from a velocity. - * This also takes in an angle parameter which is used instead of the + * integration of the pose over time. This method takes in the current time as + * a parameter to calculate period (difference between two timestamps). The + * period is used to calculate the change in distance from a velocity. This + * also takes in an angle parameter which is used instead of the * angular rate that is calculated from forward kinematics. * * @param gyroAngle The angle reported by the gyroscope. - * @param moduleStates The current state of all swerve modules. Please provide - * the states in the same order in which you instantiated - * your SwerveDriveKinematics. + * @param modulePositions The current position of all swerve modules. Please + * provide the positions in the same order in which you instantiated your + * SwerveDriveKinematics. * * @return The new pose of the robot. */ - template - const Pose2d& Update(const Rotation2d& gyroAngle, - ModuleStates&&... moduleStates) { - return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...); - } + const Pose2d& Update( + const Rotation2d& gyroAngle, + const wpi::array modulePositions); private: SwerveDriveKinematics m_kinematics; Pose2d m_pose; - units::second_t m_previousTime = -1_s; Rotation2d m_previousAngle; Rotation2d m_gyroOffset; + + wpi::array m_previousModulePositions; }; extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc index 96db9308f6..bae7cf31ea 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc @@ -11,8 +11,11 @@ namespace frc { template SwerveDriveOdometry::SwerveDriveOdometry( SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, + const wpi::array modulePositions, const Pose2d& initialPose) - : m_kinematics(kinematics), m_pose(initialPose) { + : m_kinematics(kinematics), + m_pose(initialPose), + m_previousModulePositions(modulePositions) { m_previousAngle = m_pose.Rotation(); m_gyroOffset = m_pose.Rotation() - gyroAngle; wpi::math::MathSharedStore::ReportUsage( @@ -20,24 +23,66 @@ SwerveDriveOdometry::SwerveDriveOdometry( } template -template -const Pose2d& frc::SwerveDriveOdometry::UpdateWithTime( - units::second_t currentTime, const Rotation2d& gyroAngle, - ModuleStates&&... moduleStates) { - units::second_t deltaTime = - (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s; - m_previousTime = currentTime; +template +void SwerveDriveOdometry::ResetPosition( + const Pose2d& pose, const Rotation2d& gyroAngle, + ModulePositions&&... wheelPositions) { + static_assert(sizeof...(wheelPositions) == NumModules, + "Number of modules is not consistent with number of wheel " + "locations provided in constructor."); + + wpi::array modulePositions{ + wheelPositions...}; + this->ResetPosition(pose, gyroAngle, modulePositions); +} + +template +void SwerveDriveOdometry::ResetPosition( + const Pose2d& pose, const Rotation2d& gyroAngle, + const wpi::array modulePositions) { + m_pose = pose; + m_previousAngle = pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; + m_previousModulePositions = modulePositions; +} + +template +template +const Pose2d& frc::SwerveDriveOdometry::Update( + const Rotation2d& gyroAngle, ModulePositions&&... wheelPositions) { + static_assert(sizeof...(wheelPositions) == NumModules, + "Number of modules is not consistent with number of wheel " + "locations provided in constructor."); + + wpi::array modulePositions{ + wheelPositions...}; + + return this->Update(gyroAngle, modulePositions); +} + +template +const Pose2d& frc::SwerveDriveOdometry::Update( + const Rotation2d& gyroAngle, + const wpi::array modulePositions) { + auto moduleDeltas = + wpi::array(wpi::empty_array); + for (size_t index = 0; index < modulePositions.size(); index++) { + auto lastPosition = m_previousModulePositions[index]; + auto currentPosition = modulePositions[index]; + moduleDeltas[index] = {currentPosition.distance - lastPosition.distance, + currentPosition.angle}; + } auto angle = gyroAngle + m_gyroOffset; - auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...); - static_cast(dtheta); + auto twist = m_kinematics.ToTwist2d(moduleDeltas); + twist.dtheta = (angle - m_previousAngle).Radians(); - auto newPose = m_pose.Exp( - {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()}); + auto newPose = m_pose.Exp(twist); m_previousAngle = angle; m_pose = {newPose.Translation(), angle}; + m_previousModulePositions = modulePositions; return m_pose; } diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h new file mode 100644 index 0000000000..18ed464e44 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Rotation2d.h" +#include "units/angle.h" +#include "units/length.h" +#include "units/math.h" + +namespace frc { +/** + * Represents the position of one swerve module. + */ +struct WPILIB_DLLEXPORT SwerveModulePosition { + /** + * Distance the wheel of a module has traveled + */ + units::meter_t distance = 0_m; + + /** + * Angle of the module. + */ + Rotation2d angle; +}; +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java index ab0c3af00f..24ead117a4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -6,12 +6,16 @@ package edu.wpi.first.math.estimator; import static org.junit.jupiter.api.Assertions.assertEquals; +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.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.math.numbers.N7; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import java.util.List; @@ -20,20 +24,30 @@ import org.junit.jupiter.api.Test; class SwerveDrivePoseEstimatorTest { @Test - void testAccuracy() { + void testAccuracyFacingTrajectory() { var kinematics = new SwerveDriveKinematics( new Translation2d(1, 1), new Translation2d(1, -1), new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var fl = new SwerveModulePosition(); + var fr = new SwerveModulePosition(); + var bl = new SwerveModulePosition(); + var br = new SwerveModulePosition(); + var estimator = - new SwerveDrivePoseEstimator( + new SwerveDrivePoseEstimator( + Nat.N7(), + Nat.N7(), + Nat.N5(), new Rotation2d(), new Pose2d(), + new SwerveModulePosition[] {fl, fr, bl, br}, kinematics, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.005), + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005), VecBuilder.fill(0.1, 0.1, 0.1)); var trajectory = @@ -89,6 +103,16 @@ class SwerveDrivePoseEstimatorTest { moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1; } + fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt; + fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt; + bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt; + br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt; + + fl.angle = moduleStates[0].angle; + fr.angle = moduleStates[1].angle; + bl.angle = moduleStates[2].angle; + br.angle = moduleStates[3].angle; + var xHat = estimator.updateWithTime( t, @@ -96,7 +120,129 @@ class SwerveDrivePoseEstimatorTest { .poseMeters .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05)), - moduleStates); + moduleStates, + new SwerveModulePosition[] {fl, fr, bl, br}); + + double error = + groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.125, "Incorrect max error"); + } + + @Test + void testAccuracyFacingXAxis() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + + var fl = new SwerveModulePosition(); + var fr = new SwerveModulePosition(); + var bl = new SwerveModulePosition(); + var br = new SwerveModulePosition(); + + var estimator = + new SwerveDrivePoseEstimator( + Nat.N7(), + Nat.N7(), + Nat.N5(), + new Rotation2d(), + new Pose2d(), + new SwerveModulePosition[] {fl, fr, bl, br}, + kinematics, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005), + VecBuilder.fill(0.1, 0.1, 0.1)); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(4915); + + final double dt = 0.02; + double t = 0.0; + + final double visionUpdateRate = 0.1; + Pose2d lastVisionPose = null; + double lastVisionUpdateTime = Double.NEGATIVE_INFINITY; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + if (lastVisionUpdateTime + visionUpdateRate < t) { + if (lastVisionPose != null) { + estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + } + + lastVisionPose = + new Pose2d( + new Translation2d( + groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1, + groundTruthState.poseMeters.getTranslation().getY() + + rand.nextGaussian() * 0.1), + new Rotation2d(rand.nextGaussian() * 0.1) + .plus(groundTruthState.poseMeters.getRotation())); + + lastVisionUpdateTime = t; + } + + var moduleStates = + kinematics.toSwerveModuleStates( + new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond + * groundTruthState.poseMeters.getRotation().getCos(), + groundTruthState.velocityMetersPerSecond + * groundTruthState.poseMeters.getRotation().getSin(), + 0.0)); + for (var moduleState : moduleStates) { + moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); + moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1; + } + + fl.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + fr.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + bl.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + br.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + + fl.angle = groundTruthState.poseMeters.getRotation(); + fr.angle = groundTruthState.poseMeters.getRotation(); + bl.angle = groundTruthState.poseMeters.getRotation(); + br.angle = groundTruthState.poseMeters.getRotation(); + + var xHat = + estimator.updateWithTime( + t, + new Rotation2d(rand.nextGaussian() * 0.05), + moduleStates, + new SwerveModulePosition[] {fl, fr, bl, br}); double error = groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 64abab4652..1ea41a6b70 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -41,15 +41,27 @@ class SwerveDriveKinematicsTest { @Test void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line - SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0)); + SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(0.0)); var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state); assertAll( - () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); } + @Test + void testStraightLineForwardKinematicsWithDeltas() { + // test forward kinematics going in a straight line + SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(0.0)); + var twist = m_kinematics.toTwist2d(delta, delta, delta, delta); + + assertAll( + () -> assertEquals(5.0, twist.dx, kEpsilon), + () -> assertEquals(0.0, twist.dy, kEpsilon), + () -> assertEquals(0.0, twist.dtheta, kEpsilon)); + } + @Test void testStraightStrafeInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0); @@ -77,6 +89,17 @@ class SwerveDriveKinematicsTest { () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); } + @Test + void testStraightStrafeForwardKinematicsWithDeltas() { + SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(90.0)); + var twist = m_kinematics.toTwist2d(delta, delta, delta, delta); + + assertAll( + () -> assertEquals(0.0, twist.dx, kEpsilon), + () -> assertEquals(5.0, twist.dy, kEpsilon), + () -> assertEquals(0.0, twist.dtheta, kEpsilon)); + } + @Test void testConserveWheelAngle() { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); @@ -134,6 +157,21 @@ class SwerveDriveKinematicsTest { () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)); } + @Test + void testTurnInPlaceForwardKinematicsWithDeltas() { + SwerveModulePosition flDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(135)); + SwerveModulePosition frDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(45)); + SwerveModulePosition blDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(-135)); + SwerveModulePosition brDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(-45)); + + var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); + + assertAll( + () -> assertEquals(0.0, twist.dx, kEpsilon), + () -> assertEquals(0.0, twist.dy, kEpsilon), + () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1)); + } + @Test void testOffCenterCORRotationInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); @@ -183,6 +221,30 @@ class SwerveDriveKinematicsTest { () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)); } + @Test + void testOffCenterCORRotationForwardKinematicsWithDeltas() { + SwerveModulePosition flDelta = new SwerveModulePosition(0.0, Rotation2d.fromDegrees(0.0)); + SwerveModulePosition frDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(0.0)); + SwerveModulePosition blDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(-90)); + SwerveModulePosition brDelta = new SwerveModulePosition(213.258, Rotation2d.fromDegrees(-45)); + + var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); + + /* + We already know that our omega should be 2π from the previous test. Next, we need to determine + the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, + we know that vx and vy must be the same. Furthermore, we know that the center of mass makes + a full revolution about the center of revolution once every second. Therefore, the center of + mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are + 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. + */ + + assertAll( + () -> assertEquals(75.398, twist.dx, 0.1), + () -> assertEquals(-75.398, twist.dy, 0.1), + () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1)); + } + private void assertModuleState( SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) { assertAll( @@ -247,6 +309,30 @@ class SwerveDriveKinematicsTest { () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)); } + @Test + void testOffCenterCORRotationAndTranslationForwardKinematicsWithDeltas() { + SwerveModulePosition flDelta = new SwerveModulePosition(23.43, Rotation2d.fromDegrees(-140.19)); + SwerveModulePosition frDelta = new SwerveModulePosition(23.43, Rotation2d.fromDegrees(-39.81)); + SwerveModulePosition blDelta = new SwerveModulePosition(54.08, Rotation2d.fromDegrees(-109.44)); + SwerveModulePosition brDelta = new SwerveModulePosition(54.08, Rotation2d.fromDegrees(-70.56)); + + var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); + + /* + From equation (13.17), we know that chassis motion is th dot product of the + pseudoinverse of the inverseKinematics matrix (with the center of rotation at + (0,0) -- we don't want the motion of the center of rotation, we want it of + the center of the robot). These above SwerveModuleStates are known to be from + a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been + calculated using Numpy's linalg.pinv function. + */ + + assertAll( + () -> assertEquals(0.0, twist.dx, 0.1), + () -> assertEquals(-33.0, twist.dy, 0.1), + () -> assertEquals(1.5, twist.dtheta, 0.1)); + } + @Test void testDesaturate() { SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d()); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java index cb6dfdfdb0..419942efd0 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java @@ -10,6 +10,10 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import java.util.List; +import java.util.Random; import org.junit.jupiter.api.Test; class SwerveDriveOdometryTest { @@ -18,30 +22,31 @@ class SwerveDriveOdometryTest { private final Translation2d m_bl = new Translation2d(-12, 12); private final Translation2d m_br = new Translation2d(-12, -12); + private final SwerveModulePosition zero = new SwerveModulePosition(); + private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br); private final SwerveDriveOdometry m_odometry = - new SwerveDriveOdometry(m_kinematics, new Rotation2d()); + new SwerveDriveOdometry(m_kinematics, new Rotation2d(), zero, zero, zero, zero); @Test void testTwoIterations() { // 5 units/sec in the x axis (forward) - final SwerveModuleState[] wheelSpeeds = { - new SwerveModuleState(5, Rotation2d.fromDegrees(0)), - new SwerveModuleState(5, Rotation2d.fromDegrees(0)), - new SwerveModuleState(5, Rotation2d.fromDegrees(0)), - new SwerveModuleState(5, Rotation2d.fromDegrees(0)) + final SwerveModulePosition[] wheelDeltas = { + new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)), + new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)), + new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)), + new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)) }; - m_odometry.updateWithTime( - 0.0, + m_odometry.update( new Rotation2d(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState()); - var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds); + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition()); + var pose = m_odometry.update(new Rotation2d(), wheelDeltas); assertAll( () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01), @@ -57,16 +62,16 @@ class SwerveDriveOdometryTest { // Module 2: speed 18.84955592153876 angle -90.0 // Module 3: speed 42.14888838624436 angle -26.565051177077986 - final SwerveModuleState[] wheelSpeeds = { - new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)), - new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)), - new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)), - new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565)) + final SwerveModulePosition[] wheelDeltas = { + new SwerveModulePosition(18.85, Rotation2d.fromDegrees(90.0)), + new SwerveModulePosition(42.15, Rotation2d.fromDegrees(26.565)), + new SwerveModulePosition(18.85, Rotation2d.fromDegrees(-90)), + new SwerveModulePosition(42.15, Rotation2d.fromDegrees(-26.565)) }; - final var zero = new SwerveModuleState(); + final var zero = new SwerveModulePosition(); - m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero); - final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds); + m_odometry.update(new Rotation2d(), zero, zero, zero, zero); + final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelDeltas); assertAll( () -> assertEquals(12.0, pose.getX(), 0.01), @@ -78,15 +83,169 @@ class SwerveDriveOdometryTest { void testGyroAngleReset() { var gyro = Rotation2d.fromDegrees(90.0); var fieldAngle = Rotation2d.fromDegrees(0.0); - m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro); - var state = new SwerveModuleState(); - m_odometry.updateWithTime(0.0, gyro, state, state, state, state); - state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0)); - var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state); + m_odometry.resetPosition( + new Pose2d(new Translation2d(), fieldAngle), gyro, zero, zero, zero, zero); + var delta = new SwerveModulePosition(); + m_odometry.update(gyro, delta, delta, delta, delta); + delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0)); + var pose = m_odometry.update(gyro, delta, delta, delta, delta); assertAll( () -> assertEquals(1.0, pose.getX(), 0.1), () -> assertEquals(0.00, pose.getY(), 0.1), () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)); } + + @Test + void testAccuracyFacingTrajectory() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + var odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(), zero, zero, zero, zero); + + SwerveModulePosition fl = new SwerveModulePosition(); + SwerveModulePosition fr = new SwerveModulePosition(); + SwerveModulePosition bl = new SwerveModulePosition(); + SwerveModulePosition br = new SwerveModulePosition(); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(4915); + + final double dt = 0.02; + double t = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + var moduleStates = + kinematics.toSwerveModuleStates( + new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond, + 0.0, + groundTruthState.velocityMetersPerSecond + * groundTruthState.curvatureRadPerMeter)); + for (var moduleState : moduleStates) { + moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); + moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1; + } + + fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt; + fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt; + bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt; + br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt; + + fl.angle = moduleStates[0].angle; + fr.angle = moduleStates[1].angle; + bl.angle = moduleStates[2].angle; + br.angle = moduleStates[3].angle; + + var xHat = + odometry.update( + groundTruthState + .poseMeters + .getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05)), + fl, + fr, + bl, + br); + + double error = + groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.125, "Incorrect max error"); + } + + @Test + void testAccuracyFacingXAxis() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + var odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(), zero, zero, zero, zero); + + SwerveModulePosition fl = new SwerveModulePosition(); + SwerveModulePosition fr = new SwerveModulePosition(); + SwerveModulePosition bl = new SwerveModulePosition(); + SwerveModulePosition br = new SwerveModulePosition(); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(4915); + + final double dt = 0.02; + double t = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + fl.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + fr.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + bl.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + br.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + + fl.angle = groundTruthState.poseMeters.getRotation(); + fr.angle = groundTruthState.poseMeters.getRotation(); + bl.angle = groundTruthState.poseMeters.getRotation(); + br.angle = groundTruthState.poseMeters.getRotation(); + + var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), fl, fr, bl, br); + + double error = + groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.125, "Incorrect max error"); + } } diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index 5315c7ad59..c7362ae278 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -8,20 +8,27 @@ #include "frc/estimator/SwerveDrivePoseEstimator.h" #include "frc/geometry/Pose2d.h" #include "frc/kinematics/SwerveDriveKinematics.h" -#include "frc/kinematics/SwerveDriveOdometry.h" #include "frc/trajectory/TrajectoryGenerator.h" #include "gtest/gtest.h" -TEST(SwerveDrivePoseEstimatorTest, Accuracy) { +TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) { frc::SwerveDriveKinematics<4> kinematics{ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; - frc::SwerveDrivePoseEstimator<4> estimator{ - frc::Rotation2d{}, frc::Pose2d{}, kinematics, - {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; + frc::SwerveModulePosition fl; + frc::SwerveModulePosition fr; + frc::SwerveModulePosition bl; + frc::SwerveModulePosition br; - frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d{}}; + frc::SwerveDrivePoseEstimator<4> estimator{ + frc::Rotation2d{}, + frc::Pose2d{}, + {fl, fr, bl, br}, + kinematics, + {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, + {0.05, 0.05, 0.05, 0.05, 0.05}, + {0.1, 0.1, 0.1}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -54,10 +61,9 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) { } lastVisionPose = groundTruthState.pose + - frc::Transform2d{ - frc::Translation2d{distribution(generator) * 0.1_m, - distribution(generator) * 0.1_m}, - frc::Rotation2d{distribution(generator) * 0.1 * 1_rad}}; + frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m, + distribution(generator) * 0.1_m}, + frc::Rotation2d{distribution(generator) * 0.1_rad}}; visionPoses.push_back(lastVisionPose); lastVisionUpdateTime = t; } @@ -66,11 +72,116 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) { {groundTruthState.velocity, 0_mps, groundTruthState.velocity * groundTruthState.curvature}); + fl.distance += moduleStates[0].speed * dt; + fr.distance += moduleStates[1].speed * dt; + bl.distance += moduleStates[2].speed * dt; + br.distance += moduleStates[3].speed * dt; + + fl.angle = moduleStates[0].angle; + fr.angle = moduleStates[1].angle; + bl.angle = moduleStates[2].angle; + br.angle = moduleStates[3].angle; + auto xhat = estimator.UpdateWithTime( t, groundTruthState.pose.Rotation() + frc::Rotation2d{distribution(generator) * 0.05_rad}, - moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]); + moduleStates, {fl, fr, bl, br}); + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05); + EXPECT_LT(maxError, 0.125); +} + +TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingXAxis) { + frc::SwerveDriveKinematics<4> kinematics{ + frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, + frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; + + frc::SwerveModulePosition fl; + frc::SwerveModulePosition fr; + frc::SwerveModulePosition bl; + frc::SwerveModulePosition br; + + frc::SwerveDrivePoseEstimator<4> estimator{ + frc::Rotation2d{}, + frc::Pose2d{}, + {fl, fr, bl, br}, + kinematics, + {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, + {0.05, 0.05, 0.05, 0.05, 0.05}, + {0.1, 0.1, 0.1}}; + + frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 0.02_s; + units::second_t t = 0_s; + + units::second_t kVisionUpdateRate = 0.1_s; + frc::Pose2d lastVisionPose; + units::second_t lastVisionUpdateTime{-std::numeric_limits::max()}; + + std::vector visionPoses; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + if (lastVisionUpdateTime + kVisionUpdateRate < t) { + if (lastVisionPose != frc::Pose2d{}) { + estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + } + lastVisionPose = + groundTruthState.pose + + frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m, + distribution(generator) * 0.1_m}, + frc::Rotation2d{distribution(generator) * 0.1_rad}}; + visionPoses.push_back(lastVisionPose); + lastVisionUpdateTime = t; + } + + auto moduleStates = kinematics.ToSwerveModuleStates( + {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(), + groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(), + 0_rad_per_s}); + + fl.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + fr.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + bl.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + br.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + + fl.angle = groundTruthState.pose.Rotation(); + fr.angle = groundTruthState.pose.Rotation(); + bl.angle = groundTruthState.pose.Rotation(); + br.angle = groundTruthState.pose.Rotation(); + + auto xhat = estimator.UpdateWithTime( + t, frc::Rotation2d{distribution(generator) * 0.05_rad}, moduleStates, + {fl, fr, bl, br}); double error = groundTruthState.pose.Translation() .Distance(xhat.Translation()) .value(); diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index b958b76b16..7f9c2e5121 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -49,6 +49,16 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) { EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon); } +TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) { + SwerveModulePosition delta{5.0_m, 0_deg}; + + auto twist = m_kinematics.ToTwist2d(delta, delta, delta, delta); + + EXPECT_NEAR(twist.dx.value(), 5.0, kEpsilon); + EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon); + EXPECT_NEAR(twist.dtheta.value(), 0.0, kEpsilon); +} + TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) { ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s}; auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds); @@ -73,6 +83,16 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) { EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon); } +TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) { + SwerveModulePosition delta{5_m, 90_deg}; + + auto twist = m_kinematics.ToTwist2d(delta, delta, delta, delta); + + EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon); + EXPECT_NEAR(twist.dy.value(), 5.0, kEpsilon); + EXPECT_NEAR(twist.dtheta.value(), 0.0, kEpsilon); +} + TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) { ChassisSpeeds speeds{0_mps, 0_mps, units::radians_per_second_t{2 * std::numbers::pi}}; @@ -119,6 +139,19 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) { EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon); } +TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) { + SwerveModulePosition fl{106.629_m, 135_deg}; + SwerveModulePosition fr{106.629_m, 45_deg}; + SwerveModulePosition bl{106.629_m, -135_deg}; + SwerveModulePosition br{106.629_m, -45_deg}; + + auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br); + + EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon); + EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon); + EXPECT_NEAR(twist.dtheta.value(), 2 * std::numbers::pi, kEpsilon); +} + TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) { ChassisSpeeds speeds{0_mps, 0_mps, units::radians_per_second_t{2 * std::numbers::pi}}; @@ -148,6 +181,20 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) { EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon); } +TEST_F(SwerveDriveKinematicsTest, + OffCenterCORRotationForwardKinematicsWithDeltas) { + SwerveModulePosition fl{0.0_m, 0_deg}; + SwerveModulePosition fr{150.796_m, 0_deg}; + SwerveModulePosition bl{150.796_m, -90_deg}; + SwerveModulePosition br{213.258_m, -45_deg}; + + auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br); + + EXPECT_NEAR(twist.dx.value(), 75.398, kEpsilon); + EXPECT_NEAR(twist.dy.value(), -75.398, kEpsilon); + EXPECT_NEAR(twist.dtheta.value(), 2 * std::numbers::pi, kEpsilon); +} + TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationAndTranslationInverseKinematics) { ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s}; @@ -179,6 +226,20 @@ TEST_F(SwerveDriveKinematicsTest, EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon); } +TEST_F(SwerveDriveKinematicsTest, + OffCenterCORRotationAndTranslationForwardKinematicsWithDeltas) { + SwerveModulePosition fl{23.43_m, -140.19_deg}; + SwerveModulePosition fr{23.43_m, -39.81_deg}; + SwerveModulePosition bl{54.08_m, -109.44_deg}; + SwerveModulePosition br{54.08_m, -70.56_deg}; + + auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br); + + EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon); + EXPECT_NEAR(twist.dy.value(), -33.0, kEpsilon); + EXPECT_NEAR(twist.dtheta.value(), 1.5, kEpsilon); +} + TEST_F(SwerveDriveKinematicsTest, Desaturate) { SwerveModuleState state1{5.0_mps, 0_deg}; SwerveModuleState state2{6.0_mps, 0_deg}; diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp index 582bb8aedf..ef8f0f5ae5 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp @@ -2,8 +2,14 @@ // 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 "frc/kinematics/SwerveDriveKinematics.h" #include "frc/kinematics/SwerveDriveOdometry.h" +#include "frc/trajectory/Trajectory.h" +#include "frc/trajectory/TrajectoryConfig.h" +#include "frc/trajectory/TrajectoryGenerator.h" #include "gtest/gtest.h" using namespace frc; @@ -18,18 +24,19 @@ class SwerveDriveOdometryTest : public ::testing::Test { Translation2d m_br{-12_m, -12_m}; SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br}; - SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad}; + SwerveModulePosition zero; + SwerveDriveOdometry<4> m_odometry{ + m_kinematics, 0_rad, {zero, zero, zero, zero}}; }; TEST_F(SwerveDriveOdometryTest, TwoIterations) { - SwerveModuleState state{5_mps, 0_deg}; + SwerveModulePosition position{0.5_m, 0_deg}; - m_odometry.ResetPosition(Pose2d{}, 0_rad); - m_odometry.UpdateWithTime(0_s, 0_deg, SwerveModuleState{}, - SwerveModuleState{}, SwerveModuleState{}, - SwerveModuleState{}); - auto pose = - m_odometry.UpdateWithTime(0.1_s, 0_deg, state, state, state, state); + m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero); + + m_odometry.Update(0_deg, zero, zero, zero, zero); + + auto pose = m_odometry.Update(0_deg, position, position, position, position); EXPECT_NEAR(0.5, pose.X().value(), kEpsilon); EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon); @@ -37,16 +44,13 @@ TEST_F(SwerveDriveOdometryTest, TwoIterations) { } TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) { - SwerveModuleState fl{18.85_mps, 90_deg}; - SwerveModuleState fr{42.15_mps, 26.565_deg}; - SwerveModuleState bl{18.85_mps, -90_deg}; - SwerveModuleState br{42.15_mps, -26.565_deg}; + SwerveModulePosition fl{18.85_m, 90_deg}; + SwerveModulePosition fr{42.15_m, 26.565_deg}; + SwerveModulePosition bl{18.85_m, -90_deg}; + SwerveModulePosition br{42.15_m, -26.565_deg}; - SwerveModuleState zero{0_mps, 0_deg}; - - m_odometry.ResetPosition(Pose2d{}, 0_rad); - m_odometry.UpdateWithTime(0_s, 0_deg, zero, zero, zero, zero); - auto pose = m_odometry.UpdateWithTime(1_s, 90_deg, fl, fr, bl, br); + m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero); + auto pose = m_odometry.Update(90_deg, fl, fr, bl, br); EXPECT_NEAR(12.0, pose.X().value(), kEpsilon); EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon); @@ -54,17 +58,139 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) { } TEST_F(SwerveDriveOdometryTest, GyroAngleReset) { - m_odometry.ResetPosition(Pose2d{}, 90_deg); + m_odometry.ResetPosition(Pose2d{}, 90_deg, zero, zero, zero, zero); - SwerveModuleState state{5_mps, 0_deg}; + SwerveModulePosition position{0.5_m, 0_deg}; - m_odometry.UpdateWithTime(0_s, 90_deg, SwerveModuleState{}, - SwerveModuleState{}, SwerveModuleState{}, - SwerveModuleState{}); - auto pose = - m_odometry.UpdateWithTime(0.1_s, 90_deg, state, state, state, state); + auto pose = m_odometry.Update(90_deg, position, position, position, position); EXPECT_NEAR(0.5, pose.X().value(), kEpsilon); EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon); EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon); } + +TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) { + SwerveDriveKinematics<4> kinematics{ + Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m}, + Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}}; + + SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}}; + + SwerveModulePosition fl; + SwerveModulePosition fr; + SwerveModulePosition bl; + SwerveModulePosition br; + + Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory( + std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg}, + Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg}, + Pose2d{0_m, 0_m, 45_deg}}, + TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 0.02_s; + units::second_t t = 0_s; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + Trajectory::State groundTruthState = trajectory.Sample(t); + + auto moduleStates = kinematics.ToSwerveModuleStates( + {groundTruthState.velocity, 0_mps, + groundTruthState.velocity * groundTruthState.curvature}); + + fl.distance += moduleStates[0].speed * dt; + fr.distance += moduleStates[1].speed * dt; + bl.distance += moduleStates[2].speed * dt; + br.distance += moduleStates[3].speed * dt; + + fl.angle = moduleStates[0].angle; + fr.angle = moduleStates[1].angle; + bl.angle = moduleStates[2].angle; + br.angle = moduleStates[3].angle; + + auto xhat = + odometry.Update(groundTruthState.pose.Rotation() + + frc::Rotation2d{distribution(generator) * 0.05_rad}, + fl, fr, bl, br); + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05); + EXPECT_LT(maxError, 0.125); +} + +TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) { + SwerveDriveKinematics<4> kinematics{ + Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m}, + Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}}; + + SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}}; + + SwerveModulePosition fl; + SwerveModulePosition fr; + SwerveModulePosition bl; + SwerveModulePosition br; + + Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory( + std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg}, + Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg}, + Pose2d{0_m, 0_m, 45_deg}}, + TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 0.02_s; + units::second_t t = 0_s; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + Trajectory::State groundTruthState = trajectory.Sample(t); + + fl.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + fr.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + bl.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + br.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + + fl.angle = groundTruthState.pose.Rotation(); + fr.angle = groundTruthState.pose.Rotation(); + bl.angle = groundTruthState.pose.Rotation(); + br.angle = groundTruthState.pose.Rotation(); + + auto xhat = odometry.Update( + frc::Rotation2d{distribution(generator) * 0.05_rad}, fl, fr, bl, br); + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06); + EXPECT_LT(maxError, 0.125); +}