diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java index ac158c23d4..b8ccea5ea1 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.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.MecanumDriveKinematics; import edu.wpi.first.math.kinematics.MecanumDriveOdometry; +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; @@ -43,9 +44,13 @@ class MecanumControllerCommandTest { private Rotation2d m_angle = new Rotation2d(0); private double m_frontLeftSpeed; + private double m_frontLeftDistance; private double m_rearLeftSpeed; + private double m_rearLeftDistance; private double m_frontRightSpeed; + private double m_frontRightDistance; private double m_rearRightSpeed; + private double m_rearRightDistance; private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI)); @@ -66,7 +71,10 @@ class MecanumControllerCommandTest { private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry( - m_kinematics, new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0))); + m_kinematics, + new Rotation2d(0), + new MecanumDriveWheelPositions(), + new Pose2d(0, 0, new Rotation2d(0))); public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond; @@ -75,13 +83,13 @@ class MecanumControllerCommandTest { this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond; } - public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() { - return new MecanumDriveWheelSpeeds( - m_frontLeftSpeed, m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed); + public MecanumDriveWheelPositions getCurrentWheelDistances() { + return new MecanumDriveWheelPositions( + m_frontLeftDistance, m_frontRightDistance, m_rearLeftDistance, m_rearRightDistance); } public Pose2d getRobotPose() { - m_odometry.updateWithTime(m_timer.get(), m_angle, getCurrentWheelSpeeds()); + m_odometry.update(m_angle, getCurrentWheelDistances()); return m_odometry.getPoseMeters(); } @@ -117,6 +125,10 @@ class MecanumControllerCommandTest { while (!command.isFinished()) { command.execute(); m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation(); + m_frontLeftDistance += m_frontLeftSpeed * 0.005; + m_rearLeftDistance += m_rearLeftSpeed * 0.005; + m_frontRightDistance += m_frontRightSpeed * 0.005; + m_rearRightDistance += m_rearRightSpeed * 0.005; SimHooks.stepTiming(0.005); } m_timer.stop(); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp index cb2810c63d..7f5b590c7d 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp @@ -32,9 +32,13 @@ class MecanumControllerCommandTest : public ::testing::Test { frc::Rotation2d m_angle{0_rad}; units::meters_per_second_t m_frontLeftSpeed = 0.0_mps; + units::meter_t m_frontLeftDistance = 0.0_m; units::meters_per_second_t m_rearLeftSpeed = 0.0_mps; + units::meter_t m_rearLeftDistance = 0.0_m; units::meters_per_second_t m_frontRightSpeed = 0.0_mps; + units::meter_t m_frontRightDistance = 0.0_m; units::meters_per_second_t m_rearRightSpeed = 0.0_mps; + units::meter_t m_rearRightDistance = 0.0_m; frc::ProfiledPIDController m_rotController{ 1, 0, 0, @@ -55,6 +59,7 @@ class MecanumControllerCommandTest : public ::testing::Test { frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}}; frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad, + getCurrentWheelDistances(), frc::Pose2d{0_m, 0_m, 0_rad}}; void SetUp() override { frc::sim::PauseTiming(); } @@ -66,8 +71,17 @@ class MecanumControllerCommandTest : public ::testing::Test { m_rearLeftSpeed, m_rearRightSpeed}; } + frc::MecanumDriveWheelPositions getCurrentWheelDistances() { + return frc::MecanumDriveWheelPositions{ + m_frontLeftDistance, + m_rearLeftDistance, + m_frontRightDistance, + m_rearRightDistance, + }; + } + frc::Pose2d getRobotPose() { - m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds()); + m_odometry.Update(m_angle, getCurrentWheelDistances()); return m_odometry.GetPose(); } }; @@ -105,6 +119,10 @@ TEST_F(MecanumControllerCommandTest, ReachesReference) { while (!command.IsFinished()) { command.Execute(); m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation(); + m_frontLeftDistance += m_frontLeftSpeed * 5_ms; + m_rearLeftDistance += m_rearLeftSpeed * 5_ms; + m_frontRightDistance += m_frontRightSpeed * 5_ms; + m_rearRightDistance += m_rearRightSpeed * 5_ms; frc::sim::StepTiming(5_ms); } m_timer.Stop(); diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp index b21b8dc1e5..a59b758021 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -11,6 +11,13 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const { units::meters_per_second_t{m_backRightEncoder.GetRate()}}; } +frc::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances() const { + return {units::meter_t{m_frontLeftEncoder.GetDistance()}, + units::meter_t{m_frontRightEncoder.GetDistance()}, + units::meter_t{m_backLeftEncoder.GetDistance()}, + units::meter_t{m_backRightEncoder.GetDistance()}}; +} + void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) { const auto frontLeftFeedforward = m_feedforward.Calculate(wheelSpeeds.frontLeft); @@ -52,5 +59,5 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, } void Drivetrain::UpdateOdometry() { - m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentState()); + m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentWheelDistances()); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h index 2f76b596e9..fd4ae7ae8e 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -31,6 +31,7 @@ class Drivetrain { } frc::MecanumDriveWheelSpeeds GetCurrentState() const; + frc::MecanumDriveWheelPositions GetCurrentWheelDistances() const; void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds); void Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, @@ -69,7 +70,8 @@ class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation}; - frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d()}; + frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d(), + GetCurrentWheelDistances()}; // Gains are for example purposes only - must be determined for your own // robot! diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp index b56a8e211d..9e4018354e 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -28,7 +28,8 @@ DriveSubsystem::DriveSubsystem() m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1], kRearRightEncoderReversed}, - m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d{}} { + m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), + getCurrentWheelDistances(), frc::Pose2d{}} { // Set the distance per pulse for the encoders m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); @@ -43,13 +44,7 @@ DriveSubsystem::DriveSubsystem() void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. - m_odometry.Update( - m_gyro.GetRotation2d(), - frc::MecanumDriveWheelSpeeds{ - units::meters_per_second_t{m_frontLeftEncoder.GetRate()}, - units::meters_per_second_t{m_rearLeftEncoder.GetRate()}, - units::meters_per_second_t{m_frontRightEncoder.GetRate()}, - units::meters_per_second_t{m_rearRightEncoder.GetRate()}}); + m_odometry.Update(m_gyro.GetRotation2d(), getCurrentWheelDistances()); } void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot, @@ -102,6 +97,14 @@ frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() { units::meters_per_second_t{m_rearRightEncoder.GetRate()}}); } +frc::MecanumDriveWheelPositions DriveSubsystem::getCurrentWheelDistances() { + return (frc::MecanumDriveWheelPositions{ + units::meter_t{m_frontLeftEncoder.GetDistance()}, + units::meter_t{m_rearLeftEncoder.GetDistance()}, + units::meter_t{m_frontRightEncoder.GetDistance()}, + units::meter_t{m_rearRightEncoder.GetDistance()}}); +} + void DriveSubsystem::SetMaxOutput(double maxOutput) { m_drive.SetMaxOutput(maxOutput); } @@ -123,5 +126,6 @@ frc::Pose2d DriveSubsystem::GetPose() { } void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { - m_odometry.ResetPosition(pose, m_gyro.GetRotation2d()); + m_odometry.ResetPosition(pose, m_gyro.GetRotation2d(), + getCurrentWheelDistances()); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h index 95b08b6937..579a3950ea 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h @@ -81,6 +81,13 @@ class DriveSubsystem : public frc2::SubsystemBase { */ frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds(); + /** + * Gets the distances travelled by each wheel. + * + * @return the distances travelled by each wheel. + */ + frc::MecanumDriveWheelPositions getCurrentWheelDistances(); + /** * Sets the drive MotorControllers to a desired voltage. */ diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp index b411f4aef1..38a75879c9 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp @@ -15,6 +15,13 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const { units::meters_per_second_t{m_backRightEncoder.GetRate()}}; } +frc::MecanumDriveWheelPositions Drivetrain::GetCurrentDistances() const { + return {units::meter_t{m_frontLeftEncoder.GetDistance()}, + units::meter_t{m_frontRightEncoder.GetDistance()}, + units::meter_t{m_backLeftEncoder.GetDistance()}, + units::meter_t{m_backRightEncoder.GetDistance()}}; +} + void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) { std::function @@ -49,7 +56,8 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, } void Drivetrain::UpdateOdometry() { - m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState()); + m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState(), + GetCurrentDistances()); // 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/MecanumDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h index a4cc583868..43f80933fc 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h @@ -32,6 +32,7 @@ class Drivetrain { } frc::MecanumDriveWheelSpeeds GetCurrentState() const; + frc::MecanumDriveWheelPositions GetCurrentDistances() const; void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds); void Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, @@ -75,7 +76,12 @@ class Drivetrain { // Gains are for example purposes only - must be determined for your own // robot! - frc::MecanumDrivePoseEstimator m_poseEstimator{0_deg, frc::Pose2d{}, - m_kinematics, {0.1, 0.1, 0.1}, - {0.05}, {0.1, 0.1, 0.1}}; + frc::MecanumDrivePoseEstimator m_poseEstimator{ + 0_deg, + frc::Pose2d{}, + GetCurrentDistances(), + 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/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index e6f2c18fbd..8849598ee3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.MecanumDriveKinematics; import edu.wpi.first.math.kinematics.MecanumDriveOdometry; +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; @@ -48,7 +49,7 @@ public class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); private final MecanumDriveOdometry m_odometry = - new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d()); + new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d(), getCurrentDistances()); // Gains are for example purposes only - must be determined for your own robot! private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); @@ -76,6 +77,19 @@ public class Drivetrain { m_backRightEncoder.getRate()); } + /** + * Returns the current distances measured by the drivetrain. + * + * @return The current distances measured by the drivetrain. + */ + public MecanumDriveWheelPositions getCurrentDistances() { + return new MecanumDriveWheelPositions( + m_frontLeftEncoder.getDistance(), + m_frontRightEncoder.getDistance(), + m_backLeftEncoder.getDistance(), + m_backRightEncoder.getDistance()); + } + /** * Set the desired speeds for each wheel. * @@ -126,6 +140,6 @@ public class Drivetrain { /** Updates the field relative position of the robot. */ public void updateOdometry() { - m_odometry.update(m_gyro.getRotation2d(), getCurrentState()); + m_odometry.update(m_gyro.getRotation2d(), getCurrentDistances()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java index 764f6b6546..b566f187c2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; import edu.wpi.first.math.kinematics.MecanumDriveOdometry; +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.Encoder; @@ -58,7 +59,10 @@ public class DriveSubsystem extends SubsystemBase { // Odometry class for tracking robot pose MecanumDriveOdometry m_odometry = - new MecanumDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d()); + new MecanumDriveOdometry( + DriveConstants.kDriveKinematics, + m_gyro.getRotation2d(), + new MecanumDriveWheelPositions()); /** Creates a new DriveSubsystem. */ public DriveSubsystem() { @@ -77,13 +81,7 @@ public class DriveSubsystem extends SubsystemBase { @Override public void periodic() { // Update the odometry in the periodic block - m_odometry.update( - m_gyro.getRotation2d(), - new MecanumDriveWheelSpeeds( - m_frontLeftEncoder.getRate(), - m_rearLeftEncoder.getRate(), - m_frontRightEncoder.getRate(), - m_rearRightEncoder.getRate())); + m_odometry.update(m_gyro.getRotation2d(), getCurrentWheelDistances()); } /** @@ -101,7 +99,7 @@ public class DriveSubsystem extends SubsystemBase { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition(pose, m_gyro.getRotation2d()); + m_odometry.resetPosition(pose, m_gyro.getRotation2d(), getCurrentWheelDistances()); } /** @@ -186,6 +184,19 @@ public class DriveSubsystem extends SubsystemBase { m_rearRightEncoder.getRate()); } + /** + * Gets the current wheel distance measurements. + * + * @return the current wheel distance measurements in a MecanumDriveWheelPositions object. + */ + public MecanumDriveWheelPositions getCurrentWheelDistances() { + return new MecanumDriveWheelPositions( + m_frontLeftEncoder.getDistance(), + m_rearLeftEncoder.getDistance(), + m_frontRightEncoder.getDistance(), + m_rearRightEncoder.getDistance()); + } + /** * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. * diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index e90d9463a7..7040ea1582 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -12,6 +12,7 @@ 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.MecanumDriveKinematics; +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogGyro; @@ -57,9 +58,10 @@ public class Drivetrain { new MecanumDrivePoseEstimator( m_gyro.getRotation2d(), new Pose2d(), + getCurrentDistances(), 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))); // Gains are for example purposes only - must be determined for your own robot! @@ -88,6 +90,19 @@ public class Drivetrain { m_backRightEncoder.getRate()); } + /** + * Returns the current distances measured by the drivetrain. + * + * @return The current distances measured by the drivetrain. + */ + public MecanumDriveWheelPositions getCurrentDistances() { + return new MecanumDriveWheelPositions( + m_frontLeftEncoder.getDistance(), + m_frontRightEncoder.getDistance(), + m_backLeftEncoder.getDistance(), + m_backRightEncoder.getDistance()); + } + /** * Set the desired speeds for each wheel. * @@ -138,7 +153,7 @@ public class Drivetrain { /** Updates the field relative position of the robot. */ public void updateOdometry() { - m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState()); + m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState(), getCurrentDistances()); // 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/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index dd30f30af6..e40f74ee2e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -14,9 +14,12 @@ 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.MecanumDriveKinematics; +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.math.numbers.N7; import edu.wpi.first.util.WPIUtilJNI; import java.util.function.BiConsumer; @@ -29,7 +32,7 @@ import java.util.function.BiConsumer; *

{@link MecanumDrivePoseEstimator#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 MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d, - * Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}. + * Pose2d, MecanumDriveWheelPositions, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}. * *

{@link MecanumDrivePoseEstimator#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,19 +40,21 @@ 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_fl, s_fr, s_rl, s_rr]ᵀ in the field coordinate system + * containing x position, y position, and heading, followed by the distance driven by the front + * left, front right, rear left, and rear right wheels. * - *

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_fl, v_fr, v_rl, v_rr]ᵀ containing x velocity, y + * velocity, and angular rate in the field coordinate system, followed by the velocity of the front + * left, front right, rear left, and rear right wheels. * *

y = [x, y, theta]ᵀ from vision containing x position, y position, and * heading; or y = [theta]ᵀ containing gyro heading. */ public class MecanumDrivePoseEstimator { - private final UnscentedKalmanFilter m_observer; + private final UnscentedKalmanFilter m_observer; private final MecanumDriveKinematics m_kinematics; - private final BiConsumer, Matrix> m_visionCorrect; + private final BiConsumer, Matrix> m_visionCorrect; private final TimeInterpolatableBuffer m_poseBuffer; private final double m_nominalDt; // Seconds @@ -65,13 +70,14 @@ public class MecanumDrivePoseEstimator { * * @param gyroAngle The current gyro angle. * @param initialPoseMeters The starting pose estimate. + * @param wheelPositions The distances driven by each wheel. * @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_fl, s_fr, s_rl, + * s_rr]ᵀ, with units in meters and radians, followed by 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. + * to trust sensor readings from the gyro less. This matrix is in the form [theta, s_fl, s_fr, + * s_rl, s_rr], 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. @@ -79,13 +85,15 @@ public class MecanumDrivePoseEstimator { public MecanumDrivePoseEstimator( Rotation2d gyroAngle, Pose2d initialPoseMeters, + MecanumDriveWheelPositions wheelPositions, MecanumDriveKinematics kinematics, - Matrix stateStdDevs, - Matrix localMeasurementStdDevs, + Matrix stateStdDevs, + Matrix localMeasurementStdDevs, Matrix visionMeasurementStdDevs) { this( gyroAngle, initialPoseMeters, + wheelPositions, kinematics, stateStdDevs, localMeasurementStdDevs, @@ -98,13 +106,14 @@ public class MecanumDrivePoseEstimator { * * @param gyroAngle The current gyro angle. * @param initialPoseMeters The starting pose estimate. + * @param wheelPositions The distances driven by each wheel. * @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 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. + * model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl, + * s_rr]ᵀ, with units in meters and radians, followed by 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, s_fl, s_fr, + * s_rl, s_rr], 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. @@ -113,19 +122,20 @@ public class MecanumDrivePoseEstimator { public MecanumDrivePoseEstimator( Rotation2d gyroAngle, Pose2d initialPoseMeters, + MecanumDriveWheelPositions wheelPositions, MecanumDriveKinematics kinematics, - Matrix stateStdDevs, - Matrix localMeasurementStdDevs, + Matrix stateStdDevs, + Matrix localMeasurementStdDevs, Matrix visionMeasurementStdDevs, double nominalDtSeconds) { m_nominalDt = nominalDtSeconds; m_observer = new UnscentedKalmanFilter<>( - Nat.N3(), - Nat.N1(), + Nat.N7(), + Nat.N5(), (x, u) -> u, - (x, u) -> x.extractRowVector(2), + (x, u) -> x.block(Nat.N5(), Nat.N1(), 2, 0), stateStdDevs, localMeasurementStdDevs, AngleStatistics.angleMean(2), @@ -146,7 +156,7 @@ public class MecanumDrivePoseEstimator { Nat.N3(), u, y, - (x, u1) -> x, + (x, u1) -> x.block(Nat.N3(), Nat.N1(), 0, 0), m_visionContR, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), @@ -155,7 +165,19 @@ public class MecanumDrivePoseEstimator { m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle); m_previousAngle = initialPoseMeters.getRotation(); - m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters)); + + var poseVec = StateSpaceUtil.poseTo3dVector(initialPoseMeters); + var xhat = + VecBuilder.fill( + poseVec.get(0, 0), + poseVec.get(1, 0), + poseVec.get(2, 0), + wheelPositions.frontLeftMeters, + wheelPositions.frontRightMeters, + wheelPositions.rearLeftMeters, + wheelPositions.rearRightMeters); + + m_observer.setXhat(xhat); } /** @@ -181,13 +203,26 @@ public class MecanumDrivePoseEstimator { * * @param poseMeters The position on the field that your robot is at. * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances driven by each wheel. */ - public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { + public void resetPosition( + Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) { // Reset state estimate and error covariance m_observer.reset(); m_poseBuffer.clear(); - m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters)); + var poseVec = StateSpaceUtil.poseTo3dVector(poseMeters); + var xhat = + VecBuilder.fill( + poseVec.get(0, 0), + poseVec.get(1, 0), + poseVec.get(2, 0), + wheelPositions.frontLeftMeters, + wheelPositions.frontRightMeters, + wheelPositions.rearLeftMeters, + wheelPositions.rearRightMeters); + + m_observer.setXhat(xhat); m_prevTimeSeconds = -1; @@ -227,7 +262,7 @@ public class MecanumDrivePoseEstimator { 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 MatBuilder<>(Nat.N7(), Nat.N1()).fill(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), StateSpaceUtil.poseTo3dVector( getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get())))); } @@ -273,10 +308,14 @@ public class MecanumDrivePoseEstimator { * * @param gyroAngle The current gyro angle. * @param wheelSpeeds The current speeds of the mecanum drive wheels. + * @param wheelPositions The distances driven by each wheel. * @return The estimated pose of the robot in meters. */ - public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { - return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds); + public Pose2d update( + Rotation2d gyroAngle, + MecanumDriveWheelSpeeds wheelSpeeds, + MecanumDriveWheelPositions wheelPositions) { + return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds, wheelPositions); } /** @@ -287,10 +326,14 @@ public class MecanumDrivePoseEstimator { * @param currentTimeSeconds Time at which this method was called, in seconds. * @param gyroAngle The current gyroscope angle. * @param wheelSpeeds The current speeds of the mecanum drive wheels. + * @param wheelPositions The distances driven by each wheel. * @return The estimated pose of the robot in meters. */ public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { + double currentTimeSeconds, + Rotation2d gyroAngle, + MecanumDriveWheelSpeeds wheelSpeeds, + MecanumDriveWheelPositions wheelPositions) { double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt; m_prevTimeSeconds = currentTimeSeconds; @@ -302,10 +345,24 @@ public class MecanumDrivePoseEstimator { new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) .rotateBy(angle); - var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega); + var u = + VecBuilder.fill( + fieldRelativeVelocities.getX(), + fieldRelativeVelocities.getY(), + omega, + wheelSpeeds.frontLeftMetersPerSecond, + wheelSpeeds.frontRightMetersPerSecond, + wheelSpeeds.rearLeftMetersPerSecond, + wheelSpeeds.rearRightMetersPerSecond); m_previousAngle = angle; - var localY = VecBuilder.fill(angle.getRadians()); + var localY = + VecBuilder.fill( + angle.getRadians(), + wheelPositions.frontLeftMeters, + wheelPositions.frontRightMeters, + wheelPositions.rearLeftMeters, + wheelPositions.rearRightMeters); 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/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index 0807aba1c6..5090423524 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -7,6 +7,7 @@ package edu.wpi.first.math.kinematics; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import org.ejml.simple.SimpleMatrix; /** @@ -153,6 +154,28 @@ public class MecanumDriveKinematics { chassisSpeedsVector.get(2, 0)); } + /** + * Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This + * method is often used for odometry -- determining the robot's position on the field using + * changes in the distance driven by each wheel on the robot. + * + * @param wheelDeltas The distances driven by each wheel. + * @return The resulting Twist2d. + */ + public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) { + var wheelDeltasVector = new SimpleMatrix(4, 1); + wheelDeltasVector.setColumn( + 0, + 0, + wheelDeltas.frontLeftMeters, + wheelDeltas.frontRightMeters, + wheelDeltas.rearLeftMeters, + wheelDeltas.rearRightMeters); + var twist = m_forwardKinematics.mult(wheelDeltasVector); + + return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0)); + } + /** * Construct inverse kinematics matrix from wheel locations. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java index b3c79c99a7..f04da47c34 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.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 mecanum drive odometry. Odometry allows you to track the robot's position on the field @@ -21,7 +19,7 @@ import edu.wpi.first.util.WPIUtilJNI; public class MecanumDriveOdometry { private final MecanumDriveKinematics m_kinematics; private Pose2d m_poseMeters; - private double m_prevTimeSeconds = -1; + private MecanumDriveWheelPositions m_previousWheelPositions; private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; @@ -31,14 +29,24 @@ public class MecanumDriveOdometry { * * @param kinematics The mecanum drive kinematics for your drivetrain. * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances driven by each wheel. * @param initialPoseMeters The starting position of the robot on the field. */ public MecanumDriveOdometry( - MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) { + MecanumDriveKinematics kinematics, + Rotation2d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose2d initialPoseMeters) { m_kinematics = kinematics; m_poseMeters = initialPoseMeters; m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_previousAngle = initialPoseMeters.getRotation(); + m_previousWheelPositions = + new MecanumDriveWheelPositions( + wheelPositions.frontLeftMeters, + wheelPositions.frontRightMeters, + wheelPositions.rearLeftMeters, + wheelPositions.rearRightMeters); MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1); } @@ -47,9 +55,13 @@ public class MecanumDriveOdometry { * * @param kinematics The mecanum drive kinematics for your drivetrain. * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances driven by each wheel. */ - public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) { - this(kinematics, gyroAngle, new Pose2d()); + public MecanumDriveOdometry( + MecanumDriveKinematics kinematics, + Rotation2d gyroAngle, + MecanumDriveWheelPositions wheelPositions) { + this(kinematics, gyroAngle, wheelPositions, new Pose2d()); } /** @@ -60,11 +72,19 @@ public class MecanumDriveOdometry { * * @param poseMeters The position on the field that your robot is at. * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances driven by each wheel. */ - public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { + public void resetPosition( + Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) { m_poseMeters = poseMeters; m_previousAngle = poseMeters.getRotation(); m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + m_previousWheelPositions = + new MecanumDriveWheelPositions( + wheelPositions.frontLeftMeters, + wheelPositions.frontRightMeters, + wheelPositions.rearLeftMeters, + wheelPositions.rearRightMeters); } /** @@ -78,48 +98,37 @@ public class MecanumDriveOdometry { /** * 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. + * over time. This method takes in an angle parameter which is used instead of the angular rate + * that is calculated from forward kinematics, in addition to the current distance measurement at + * each wheel. * - * @param currentTimeSeconds The current time in seconds. * @param gyroAngle The angle reported by the gyroscope. - * @param wheelSpeeds The current wheel speeds. + * @param wheelPositions The distances driven by each wheel. * @return The new pose of the robot. */ - public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { - double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; - m_prevTimeSeconds = currentTimeSeconds; - + public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) { var angle = gyroAngle.plus(m_gyroOffset); - var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds); - var newPose = - m_poseMeters.exp( - new Twist2d( - chassisState.vxMetersPerSecond * period, - chassisState.vyMetersPerSecond * period, - angle.minus(m_previousAngle).getRadians())); + var wheelDeltas = + new MecanumDriveWheelPositions( + wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters, + wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters, + wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters, + wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters); + + var twist = m_kinematics.toTwist2d(wheelDeltas); + twist.dtheta = angle.minus(m_previousAngle).getRadians(); + var newPose = m_poseMeters.exp(twist); m_previousAngle = angle; m_poseMeters = new Pose2d(newPose.getTranslation(), angle); + m_previousWheelPositions = + new MecanumDriveWheelPositions( + wheelPositions.frontLeftMeters, + wheelPositions.frontRightMeters, + wheelPositions.rearLeftMeters, + wheelPositions.rearRightMeters); + 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 - * (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 wheelSpeeds The current wheel speeds. - * @return The new pose of the robot. - */ - public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { - return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds); - } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java new file mode 100644 index 0000000000..9ff3341682 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java @@ -0,0 +1,49 @@ +// 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; + +public class MecanumDriveWheelPositions { + /** Distance measured by the front left wheel. */ + public double frontLeftMeters; + + /** Distance measured by the front right wheel. */ + public double frontRightMeters; + + /** Distance measured by the rear left wheel. */ + public double rearLeftMeters; + + /** Distance measured by the rear right wheel. */ + public double rearRightMeters; + + /** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */ + public MecanumDriveWheelPositions() {} + + /** + * Constructs a MecanumDriveWheelPositions. + * + * @param frontLeftMeters Distance measured by the front left wheel. + * @param frontRightMeters Distance measured by the front right wheel. + * @param rearLeftMeters Distance measured by the rear left wheel. + * @param rearRightMeters Distance measured by the rear right wheel. + */ + public MecanumDriveWheelPositions( + double frontLeftMeters, + double frontRightMeters, + double rearLeftMeters, + double rearRightMeters) { + this.frontLeftMeters = frontLeftMeters; + this.frontRightMeters = frontRightMeters; + this.rearLeftMeters = rearLeftMeters; + this.rearRightMeters = rearRightMeters; + } + + @Override + public String toString() { + return String.format( + "MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, " + + "Rear Left: %.2f m, Rear Right: %.2f m)", + frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters); + } +} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index cb175e8541..e6975309ed 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -13,32 +13,47 @@ using namespace frc; frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, + const MecanumDriveWheelPositions& wheelPositions, MecanumDriveKinematics 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) - : 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<7>& x, const Vectord<7>& u) { return u; }, + [](const Vectord<7>& x, const Vectord<7>& u) { + return x.block<5, 1>(2, 0); }, - stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2), - frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2), - frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt), + stateStdDevs, localMeasurementStdDevs, frc::AngleMean<7, 7>(2), + frc::AngleMean<5, 7>(0), frc::AngleResidual<7>(2), + frc::AngleResidual<5>(0), frc::AngleAdd<7>(2), nominalDt), m_kinematics(kinematics), m_nominalDt(nominalDt) { SetVisionMeasurementStdDevs(visionMeasurementStdDevs); // Create vision correction mechanism. - m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) { + m_visionCorrect = [&](const Vectord<7>& u, const Vectord<3>& y) { m_observer.Correct<3>( - u, y, [](const Vectord<3>& x, const Vectord<3>&) { return x; }, - m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), - frc::AngleResidual<3>(2), frc::AngleAdd<3>(2)); + u, y, + [](const Vectord<7>& x, const Vectord<7>& u) { + return x.template block<3, 1>(0, 0); + }, + m_visionContR, frc::AngleMean<3, 7>(2), frc::AngleResidual<3>(2), + frc::AngleResidual<7>(2), frc::AngleAdd<7>(2)); }; // Set initial state. - m_observer.SetXhat(PoseTo3dVector(initialPose)); + auto poseVec = PoseTo3dVector(initialPose); + auto xhat = Vectord<7>{ + poseVec(0), + poseVec(1), + poseVec(2), + wheelPositions.frontLeft.value(), + wheelPositions.frontRight.value(), + wheelPositions.rearLeft.value(), + wheelPositions.rearRight.value(), + }; + + m_observer.SetXhat(xhat); // Calculate offsets. m_gyroOffset = initialPose.Rotation() - gyroAngle; @@ -52,12 +67,24 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs( } void frc::MecanumDrivePoseEstimator::ResetPosition( - const Pose2d& pose, const Rotation2d& gyroAngle) { + const Pose2d& pose, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions) { // Reset state estimate and error covariance m_observer.Reset(); m_poseBuffer.Clear(); - m_observer.SetXhat(PoseTo3dVector(pose)); + auto poseVec = PoseTo3dVector(pose); + auto xhat = Vectord<7>{ + poseVec(0), + poseVec(1), + poseVec(2), + wheelPositions.frontLeft.value(), + wheelPositions.frontRight.value(), + wheelPositions.rearLeft.value(), + wheelPositions.rearRight.value(), + }; + + m_observer.SetXhat(xhat); m_prevTime = -1_s; @@ -73,21 +100,23 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const { void frc::MecanumDrivePoseEstimator::AddVisionMeasurement( const Pose2d& visionRobotPose, units::second_t timestamp) { if (auto sample = m_poseBuffer.Sample(timestamp)) { - m_visionCorrect(Vectord<3>::Zero(), + m_visionCorrect(Vectord<7>::Zero(), PoseTo3dVector(GetEstimatedPosition().TransformBy( visionRobotPose - sample.value()))); } } Pose2d frc::MecanumDrivePoseEstimator::Update( - const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) { + const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds, + const MecanumDriveWheelPositions& wheelPositions) { return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle, - wheelSpeeds); + wheelSpeeds, wheelPositions); } Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( units::second_t currentTime, const Rotation2d& gyroAngle, - const MecanumDriveWheelSpeeds& wheelSpeeds) { + const MecanumDriveWheelSpeeds& wheelSpeeds, + const MecanumDriveWheelPositions& wheelPositions) { auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt; m_prevTime = currentTime; @@ -99,10 +128,18 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( Translation2d{chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s}.RotateBy( angle); - Vectord<3> u{fieldRelativeVelocities.X().value(), - fieldRelativeVelocities.Y().value(), omega.value()}; + Vectord<7> u{fieldRelativeVelocities.X().value(), + fieldRelativeVelocities.Y().value(), + omega.value(), + wheelSpeeds.frontLeft.value(), + wheelSpeeds.frontRight.value(), + wheelSpeeds.rearLeft.value(), + wheelSpeeds.rearRight.value()}; - Vectord<1> localY{angle.Radians().value()}; + Vectord<5> localY{angle.Radians().value(), wheelPositions.frontLeft.value(), + wheelPositions.frontRight.value(), + wheelPositions.rearLeft.value(), + wheelPositions.rearRight.value()}; m_previousAngle = angle; m_poseBuffer.AddSample(currentTime, GetEstimatedPosition()); diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index cf13c9f0e3..298dd7f48a 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -49,6 +49,18 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds( units::radians_per_second_t{chassisSpeedsVector(2)}}; } +Twist2d MecanumDriveKinematics::ToTwist2d( + const MecanumDriveWheelPositions& wheelDeltas) const { + Vectord<4> wheelDeltasVector{ + wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(), + wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()}; + + Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector); + + return {units::meter_t{twistVector(0)}, // NOLINT + units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}}; +} + void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl, Translation2d fr, Translation2d rl, diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp index bbeee582af..329966fb16 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp @@ -8,32 +8,37 @@ using namespace frc; -MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics, - const Rotation2d& gyroAngle, - const Pose2d& initialPose) - : m_kinematics(kinematics), m_pose(initialPose) { +MecanumDriveOdometry::MecanumDriveOdometry( + MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions wheelPositions, const Pose2d& initialPose) + : m_kinematics(kinematics), + m_pose(initialPose), + m_previousWheelPositions(wheelPositions) { m_previousAngle = m_pose.Rotation(); m_gyroOffset = m_pose.Rotation() - gyroAngle; wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); } -const Pose2d& MecanumDriveOdometry::UpdateWithTime( - units::second_t currentTime, const Rotation2d& gyroAngle, - MecanumDriveWheelSpeeds wheelSpeeds) { - units::second_t deltaTime = - (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s; - m_previousTime = currentTime; - +const Pose2d& MecanumDriveOdometry::Update( + const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions wheelPositions) { auto angle = gyroAngle + m_gyroOffset; - auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds); - static_cast(dtheta); + MecanumDriveWheelPositions wheelDeltas{ + wheelPositions.frontLeft - m_previousWheelPositions.frontLeft, + wheelPositions.frontRight - m_previousWheelPositions.frontRight, + wheelPositions.rearLeft - m_previousWheelPositions.rearLeft, + wheelPositions.rearRight - m_previousWheelPositions.rearRight, + }; - auto newPose = m_pose.Exp( - {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()}); + auto twist = m_kinematics.ToTwist2d(wheelDeltas); + twist.dtheta = (angle - m_previousAngle).Radians(); + + auto newPose = m_pose.Exp(twist); m_previousAngle = angle; + m_previousWheelPositions = wheelPositions; m_pose = {newPose.Translation(), angle}; return m_pose; diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 5008280986..44e40a9afa 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -35,11 +35,15 @@ 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_fl, s_fr, s_rl, s_rr]ᵀ in the field + * coordinate system containing x position, y position, and heading, followed + * by the distance driven by the front left, front right, rear left, and rear + * right wheels. * - * 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_fl, v_fr, v_rl, v_rr]ᵀ containing + * x velocity, y velocity, and angular rate in the field coordinate system, + * followed by the velocity of the front left, front right, rear left, and rear + * right wheels. * * y = [x, y, theta]ᵀ from vision containing x position, y * position, and heading; or y = [theta]ᵀ containing gyro @@ -52,17 +56,21 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { * * @param gyroAngle The current gyro angle. * @param initialPose The starting pose estimate. + * @param wheelPositions The distance measured by each wheel. * @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. + * is in the form [x, y, theta, s_fl, s_fr, + * s_rl, s_rr]ᵀ, with units in meters and + * radians, followed by 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, s_fl, s_fr, + * s_rl, s_rr], with units in radians, + * followed by meters. * @param visionMeasurementStdDevs Standard deviations of the vision * measurements. Increase these numbers to * trust global measurements from vision @@ -74,9 +82,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { */ MecanumDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, + const MecanumDriveWheelPositions& wheelPositions, MecanumDriveKinematics 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); @@ -105,8 +114,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { * * @param pose The position on the field that your robot is at. * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The distances measured at each wheel. */ - void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle); + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions); /** * Gets the pose of the robot at the current time as estimated by the Extended @@ -190,10 +201,12 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { * * @param gyroAngle The current gyro angle. * @param wheelSpeeds The current speeds of the mecanum drive wheels. + * @param wheelPositions The distances measured at each wheel. * @return The estimated pose of the robot in meters. */ Pose2d Update(const Rotation2d& gyroAngle, - const MecanumDriveWheelSpeeds& wheelSpeeds); + const MecanumDriveWheelSpeeds& wheelSpeeds, + const MecanumDriveWheelPositions& wheelPositions); /** * Updates the the Unscented Kalman Filter using only wheel encoder @@ -203,17 +216,19 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { * @param currentTime Time at which this method was called, in seconds. * @param gyroAngle The current gyroscope angle. * @param wheelSpeeds The current speeds of the mecanum drive wheels. + * @param wheelPositions The distances measured at each wheel. * @return The estimated pose of the robot in meters. */ Pose2d UpdateWithTime(units::second_t currentTime, const Rotation2d& gyroAngle, - const MecanumDriveWheelSpeeds& wheelSpeeds); + const MecanumDriveWheelSpeeds& wheelSpeeds, + const MecanumDriveWheelPositions& wheelPositions); private: - UnscentedKalmanFilter<3, 3, 1> m_observer; + UnscentedKalmanFilter<7, 7, 5> m_observer; MecanumDriveKinematics 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/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index 345d446d90..2880cef9e6 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -9,7 +9,9 @@ #include "Eigen/QR" #include "frc/EigenCore.h" #include "frc/geometry/Translation2d.h" +#include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/MecanumDriveWheelPositions.h" #include "frc/kinematics/MecanumDriveWheelSpeeds.h" #include "wpimath/MathShared.h" @@ -114,6 +116,18 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics { ChassisSpeeds ToChassisSpeeds( const MecanumDriveWheelSpeeds& wheelSpeeds) const; + /** + * Performs forward kinematics to return the resulting Twist2d from the + * given wheel position deltas. This method is often used for odometry -- + * determining the robot's position on the field using data from the + * distance driven by each wheel on the robot. + * + * @param wheelDeltas The change in distance driven by each wheel. + * + * @return The resulting chassis speed. + */ + Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const; + private: mutable Matrixd<4, 3> m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h index f49ec7b9a1..1d48cdd4a8 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h @@ -30,10 +30,12 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry { * * @param kinematics The mecanum drive kinematics for your drivetrain. * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. * @param initialPose The starting position of the robot on the field. */ explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions wheelPositions, const Pose2d& initialPose = Pose2d{}); /** @@ -44,11 +46,14 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry { * * @param pose The position on the field that your robot is at. * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. */ - void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) { + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle, + const MecanumDriveWheelPositions wheelPositions) { m_pose = pose; m_previousAngle = pose.Rotation(); m_gyroOffset = m_pose.Rotation() - gyroAngle; + m_previousWheelPositions = wheelPositions; } /** @@ -59,45 +64,23 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry { /** * 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 currentTime The current time. - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelSpeeds The current wheel speeds. - * - * @return The new pose of the robot. - */ - const Pose2d& UpdateWithTime(units::second_t currentTime, - const Rotation2d& gyroAngle, - MecanumDriveWheelSpeeds wheelSpeeds); - - /** - * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This method automatically calculates - * the current time to calculate period (difference between two timestamps). - * The period is used to calculate the change in distance from a velocity. - * This also takes in an angle parameter which is used instead of the - * angular rate that is calculated from forward kinematics. + * integration of the pose over time. This method takes in an angle parameter + * which is used instead of the angular rate that is calculated from forward + * kinematics, in addition to the current distance measurement at each wheel. * * @param gyroAngle The angle reported by the gyroscope. - * @param wheelSpeeds The current wheel speeds. + * @param wheelPositions The current distances measured by each wheel. * * @return The new pose of the robot. */ const Pose2d& Update(const Rotation2d& gyroAngle, - MecanumDriveWheelSpeeds wheelSpeeds) { - return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds); - } + const MecanumDriveWheelPositions wheelPositions); private: MecanumDriveKinematics m_kinematics; Pose2d m_pose; - units::second_t m_previousTime = -1_s; + MecanumDriveWheelPositions m_previousWheelPositions; Rotation2d m_previousAngle; Rotation2d m_gyroOffset; }; diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h new file mode 100644 index 0000000000..235d96f5a1 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "units/length.h" + +namespace frc { +/** + * Represents the wheel speeds for a mecanum drive drivetrain. + */ +struct WPILIB_DLLEXPORT MecanumDriveWheelPositions { + /** + * Distance driven by the front-left wheel. + */ + units::meter_t frontLeft = 0_m; + + /** + * Distance driven by the front-right wheel. + */ + units::meter_t frontRight = 0_m; + + /** + * Distance driven by the rear-left wheel. + */ + units::meter_t rearLeft = 0_m; + + /** + * Distance driven by the rear-right wheel. + */ + units::meter_t rearRight = 0_m; +}; +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java index d2ca3011aa..1ccd1589e4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java @@ -12,6 +12,7 @@ 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.MecanumDriveKinematics; +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import java.util.List; @@ -20,19 +21,22 @@ import org.junit.jupiter.api.Test; class MecanumDrivePoseEstimatorTest { @Test - void testAccuracy() { + void testAccuracyFacingTrajectory() { var kinematics = new MecanumDriveKinematics( new Translation2d(1, 1), new Translation2d(1, -1), new Translation2d(-1, -1), new Translation2d(-1, 1)); + var wheelPositions = new MecanumDriveWheelPositions(); + var estimator = new MecanumDrivePoseEstimator( new Rotation2d(), new Pose2d(), + wheelPositions, kinematics, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.05), + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05), VecBuilder.fill(0.1, 0.1, 0.1)); var trajectory = @@ -90,6 +94,11 @@ class MecanumDrivePoseEstimatorTest { wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; + wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; + wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; + wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + var xHat = estimator.updateWithTime( t, @@ -97,7 +106,107 @@ class MecanumDrivePoseEstimatorTest { .poseMeters .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05)), - wheelSpeeds); + wheelSpeeds, + wheelPositions); + + 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 MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var estimator = + new MecanumDrivePoseEstimator( + new Rotation2d(), + new Pose2d(), + wheelPositions, + kinematics, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05), + VecBuilder.fill(0.1, 0.1, 0.1)); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(), + new Pose2d(20, 20, Rotation2d.fromDegrees(0)), + new Pose2d(10, 10, Rotation2d.fromDegrees(180)), + new Pose2d(30, 30, Rotation2d.fromDegrees(0)), + new Pose2d(20, 20, Rotation2d.fromDegrees(180)), + new Pose2d(10, 10, Rotation2d.fromDegrees(0))), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(5190); + + 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 wheelSpeeds = + kinematics.toWheelSpeeds( + new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond + * groundTruthState.poseMeters.getRotation().getCos(), + groundTruthState.velocityMetersPerSecond + * groundTruthState.poseMeters.getRotation().getSin(), + 0)); + + wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + + wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; + wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; + wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; + wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + + var xHat = + estimator.updateWithTime( + t, new Rotation2d(rand.nextGaussian() * 0.05), wheelSpeeds, wheelPositions); double error = groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java index b3546f5cda..18d3ec7af6 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java @@ -44,6 +44,17 @@ class MecanumDriveKinematicsTest { () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)); } + @Test + void testStraightLineForwardKinematicsKinematicsWithDeltas() { + var wheelDeltas = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536); + var twist = m_kinematics.toTwist2d(wheelDeltas); + + assertAll( + () -> assertEquals(3.536, twist.dx, 0.1), + () -> assertEquals(0, twist.dy, 0.1), + () -> assertEquals(0, twist.dtheta, 0.1)); + } + @Test void testStrafeInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0); @@ -67,6 +78,17 @@ class MecanumDriveKinematicsTest { () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)); } + @Test + void testStrafeForwardKinematicsKinematicsWithDeltas() { + var wheelDeltas = new MecanumDriveWheelPositions(-2.828427, 2.828427, 2.828427, -2.828427); + var twist = m_kinematics.toTwist2d(wheelDeltas); + + assertAll( + () -> assertEquals(0, twist.dx, 0.1), + () -> assertEquals(2.8284, twist.dy, 0.1), + () -> assertEquals(0, twist.dtheta, 0.1)); + } + @Test void testRotationInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); @@ -90,6 +112,17 @@ class MecanumDriveKinematicsTest { () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)); } + @Test + void testRotationForwardKinematicsKinematicsWithDeltas() { + var wheelDeltas = new MecanumDriveWheelPositions(-150.79645, 150.79645, -150.79645, 150.79645); + var twist = m_kinematics.toTwist2d(wheelDeltas); + + assertAll( + () -> assertEquals(0, twist.dx, 0.1), + () -> assertEquals(0, twist.dy, 0.1), + () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1)); + } + @Test void testMixedTranslationRotationInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1); @@ -113,6 +146,17 @@ class MecanumDriveKinematicsTest { () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); } + @Test + void testMixedTranslationRotationForwardKinematicsKinematicsWithDeltas() { + var wheelDeltas = new MecanumDriveWheelPositions(-17.677670, 20.51, -13.44, 16.26); + var twist = m_kinematics.toTwist2d(wheelDeltas); + + assertAll( + () -> assertEquals(1.413, twist.dx, 0.1), + () -> assertEquals(2.122, twist.dy, 0.1), + () -> assertEquals(0.707, twist.dtheta, 0.1)); + } + @Test void testOffCenterRotationInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1); @@ -136,6 +180,17 @@ class MecanumDriveKinematicsTest { () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); } + @Test + void testOffCenterRotationForwardKinematicsKinematicsWithDeltas() { + var wheelDeltas = new MecanumDriveWheelPositions(0, 16.971, -16.971, 33.941); + var twist = m_kinematics.toTwist2d(wheelDeltas); + + assertAll( + () -> assertEquals(8.48525, twist.dx, 0.1), + () -> assertEquals(-8.48525, twist.dy, 0.1), + () -> assertEquals(0.707, twist.dtheta, 0.1)); + } + @Test void testOffCenterTranslationRotationInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1); @@ -159,6 +214,17 @@ class MecanumDriveKinematicsTest { () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); } + @Test + void testOffCenterRotationTranslationForwardKinematicsKinematicsWithDeltas() { + var wheelDeltas = new MecanumDriveWheelPositions(2.12, 21.92, -12.02, 36.06); + var twist = m_kinematics.toTwist2d(wheelDeltas); + + assertAll( + () -> assertEquals(12.02, twist.dx, 0.1), + () -> assertEquals(-7.07, twist.dy, 0.1), + () -> assertEquals(0.707, twist.dtheta, 0.1)); + } + @Test void testDesaturate() { var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java index 3f431093f3..5ddfcdf7f3 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.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 MecanumDriveOdometryTest { @@ -21,15 +25,19 @@ class MecanumDriveOdometryTest { private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br); + private final MecanumDriveWheelPositions zero = new MecanumDriveWheelPositions(); + private final MecanumDriveOdometry m_odometry = - new MecanumDriveOdometry(m_kinematics, new Rotation2d()); + new MecanumDriveOdometry(m_kinematics, new Rotation2d(), zero); @Test void testMultipleConsecutiveUpdates() { - var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536); + var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536); - m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds); - var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds); + m_odometry.resetPosition(new Pose2d(), new Rotation2d(), wheelPositions); + + m_odometry.update(new Rotation2d(), wheelPositions); + var secondPose = m_odometry.update(new Rotation2d(), wheelPositions); assertAll( () -> assertEquals(secondPose.getX(), 0.0, 0.01), @@ -40,10 +48,11 @@ class MecanumDriveOdometryTest { @Test void testTwoIterations() { // 5 units/sec in the x axis (forward) - final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536); + final var wheelPositions = new MecanumDriveWheelPositions(0.3536, 0.3536, 0.3536, 0.3536); + m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions()); - m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds()); - var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds); + m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions()); + var pose = m_odometry.update(new Rotation2d(), wheelPositions); assertAll( () -> assertEquals(0.3536, pose.getX(), 0.01), @@ -55,10 +64,11 @@ class MecanumDriveOdometryTest { void test90degreeTurn() { // This is a 90 degree turn about the point between front left and rear left wheels // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946 - final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986); + final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986); + m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions()); - m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds()); - final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds); + m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions()); + final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelPositions); assertAll( () -> assertEquals(8.4855, pose.getX(), 0.01), @@ -70,14 +80,188 @@ class MecanumDriveOdometryTest { void testGyroAngleReset() { var gyro = Rotation2d.fromDegrees(90.0); var fieldAngle = Rotation2d.fromDegrees(0.0); - m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro); - var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536); - m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds()); - var pose = m_odometry.updateWithTime(1.0, gyro, speeds); + m_odometry.resetPosition( + new Pose2d(new Translation2d(), fieldAngle), gyro, new MecanumDriveWheelPositions()); + var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536); + m_odometry.update(gyro, new MecanumDriveWheelPositions()); + var pose = m_odometry.update(gyro, speeds); assertAll( () -> assertEquals(3.536, pose.getX(), 0.1), () -> assertEquals(0.0, pose.getY(), 0.1), () -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1)); } + + @Test + void testAccuracyFacingTrajectory() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var odometry = + new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d()); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(), + new Pose2d(20, 20, Rotation2d.fromDegrees(0)), + new Pose2d(10, 10, Rotation2d.fromDegrees(180)), + new Pose2d(30, 30, Rotation2d.fromDegrees(0)), + new Pose2d(20, 20, Rotation2d.fromDegrees(180)), + new Pose2d(10, 10, Rotation2d.fromDegrees(0))), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(5190); + + final double dt = 0.02; + double t = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + double odometryDistanceTravelled = 0; + double trajectoryDistanceTravelled = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + trajectoryDistanceTravelled += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + + var wheelSpeeds = + kinematics.toWheelSpeeds( + new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond, + 0, + groundTruthState.velocityMetersPerSecond + * groundTruthState.curvatureRadPerMeter)); + + wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + + wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; + wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; + wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; + wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + + var lastPose = odometry.getPoseMeters(); + + var xHat = + odometry.update( + groundTruthState + .poseMeters + .getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05)), + wheelPositions); + + odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation()); + + 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.15, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.3, "Incorrect max error"); + assertEquals( + 1.0, + odometryDistanceTravelled / trajectoryDistanceTravelled, + 0.05, + "Incorrect distance travelled"); + } + + @Test + void testAccuracyFacingXAxis() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var odometry = + new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d()); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(), + new Pose2d(20, 20, Rotation2d.fromDegrees(0)), + new Pose2d(10, 10, Rotation2d.fromDegrees(180)), + new Pose2d(30, 30, Rotation2d.fromDegrees(0)), + new Pose2d(20, 20, Rotation2d.fromDegrees(180)), + new Pose2d(10, 10, Rotation2d.fromDegrees(0))), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(5190); + + final double dt = 0.02; + double t = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + double odometryDistanceTravelled = 0; + double trajectoryDistanceTravelled = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + trajectoryDistanceTravelled += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + + var wheelSpeeds = + kinematics.toWheelSpeeds( + new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond + * groundTruthState.poseMeters.getRotation().getCos(), + groundTruthState.velocityMetersPerSecond + * groundTruthState.poseMeters.getRotation().getSin(), + 0)); + + wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + + wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; + wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; + wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; + wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + + var lastPose = odometry.getPoseMeters(); + + var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), wheelPositions); + + odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation()); + + 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.15, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.3, "Incorrect max error"); + assertEquals( + 1.0, + odometryDistanceTravelled / trajectoryDistanceTravelled, + 0.05, + "Incorrect distance travelled"); + } } diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index 0845eef3c5..ff35ad68dc 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -8,20 +8,23 @@ #include "frc/estimator/MecanumDrivePoseEstimator.h" #include "frc/geometry/Pose2d.h" #include "frc/kinematics/MecanumDriveKinematics.h" -#include "frc/kinematics/MecanumDriveOdometry.h" #include "frc/trajectory/TrajectoryGenerator.h" #include "gtest/gtest.h" -TEST(MecanumDrivePoseEstimatorTest, Accuracy) { +TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) { frc::MecanumDriveKinematics 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::MecanumDrivePoseEstimator estimator{ - frc::Rotation2d{}, frc::Pose2d{}, kinematics, - {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; + frc::MecanumDriveWheelPositions wheelPositions; - frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{}}; + frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{}, + frc::Pose2d{}, + wheelPositions, + 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}, @@ -66,11 +69,99 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) { {groundTruthState.velocity, 0_mps, groundTruthState.velocity * groundTruthState.curvature}); + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; + auto xhat = estimator.UpdateWithTime( t, groundTruthState.pose.Rotation() + frc::Rotation2d{distribution(generator) * 0.05_rad}, - wheelSpeeds); + wheelSpeeds, wheelPositions); + 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(MecanumDrivePoseEstimatorTest, AccuracyFacingXAxis) { + frc::MecanumDriveKinematics 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::MecanumDriveWheelPositions wheelPositions; + + frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{}, + frc::Pose2d{}, + wheelPositions, + 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 * 1_rad}}; + visionPoses.push_back(lastVisionPose); + lastVisionUpdateTime = t; + } + + auto wheelSpeeds = kinematics.ToWheelSpeeds( + {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(), + groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(), + 0_rad_per_s}); + + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; + + auto xhat = estimator.UpdateWithTime( + t, frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelSpeeds, + wheelPositions); double error = groundTruthState.pose.Translation() .Distance(xhat.Translation()) .value(); diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index 91c88de40f..364163ee61 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -40,6 +40,15 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) { EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1); } +TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) { + MecanumDriveWheelPositions wheelDeltas{5_m, 5_m, 5_m, 5_m}; + auto twist = kinematics.ToTwist2d(wheelDeltas); + + EXPECT_NEAR(5.0, twist.dx.value(), 0.1); + EXPECT_NEAR(0.0, twist.dy.value(), 0.1); + EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1); +} + TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) { ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s}; auto moduleStates = kinematics.ToWheelSpeeds(speeds); @@ -59,6 +68,15 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) { EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1); } +TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) { + MecanumDriveWheelPositions wheelDeltas{-5_m, 5_m, 5_m, -5_m}; + auto twist = kinematics.ToTwist2d(wheelDeltas); + + EXPECT_NEAR(0.0, twist.dx.value(), 0.1); + EXPECT_NEAR(5.0, twist.dy.value(), 0.1); + EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1); +} + TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) { ChassisSpeeds speeds{0_mps, 0_mps, units::radians_per_second_t{2 * std::numbers::pi}}; @@ -80,6 +98,16 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) { EXPECT_NEAR(2 * std::numbers::pi, chassisSpeeds.omega.value(), 0.1); } +TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) { + MecanumDriveWheelPositions wheelDeltas{-150.79644737_m, 150.79644737_m, + -150.79644737_m, 150.79644737_m}; + auto twist = kinematics.ToTwist2d(wheelDeltas); + + EXPECT_NEAR(0.0, twist.dx.value(), 0.1); + EXPECT_NEAR(0.0, twist.dy.value(), 0.1); + EXPECT_NEAR(2 * std::numbers::pi, twist.dtheta.value(), 0.1); +} + TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) { ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s}; auto moduleStates = kinematics.ToWheelSpeeds(speeds); @@ -101,6 +129,18 @@ TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) { EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1); } +TEST_F(MecanumDriveKinematicsTest, + MixedRotationTranslationForwardKinematicsWithDeltas) { + MecanumDriveWheelPositions wheelDeltas{-17.677670_m, 20.506097_m, -13.435_m, + 16.26_m}; + + auto twist = kinematics.ToTwist2d(wheelDeltas); + + EXPECT_NEAR(1.41335, twist.dx.value(), 0.1); + EXPECT_NEAR(2.1221, twist.dy.value(), 0.1); + EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1); +} + TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) { ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s}; auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl); @@ -121,6 +161,16 @@ TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) { EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1); } +TEST_F(MecanumDriveKinematicsTest, + OffCenterRotationForwardKinematicsWithDeltas) { + MecanumDriveWheelPositions wheelDeltas{0_m, 16.971_m, -16.971_m, 33.941_m}; + auto twist = kinematics.ToTwist2d(wheelDeltas); + + EXPECT_NEAR(8.48525, twist.dx.value(), 0.1); + EXPECT_NEAR(-8.48525, twist.dy.value(), 0.1); + EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1); +} + TEST_F(MecanumDriveKinematicsTest, OffCenterTranslationRotationInverseKinematics) { ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s}; @@ -143,6 +193,16 @@ TEST_F(MecanumDriveKinematicsTest, EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1); } +TEST_F(MecanumDriveKinematicsTest, + OffCenterTranslationRotationForwardKinematicsWithDeltas) { + MecanumDriveWheelPositions wheelDeltas{2.12_m, 21.92_m, -12.02_m, 36.06_m}; + auto twist = kinematics.ToTwist2d(wheelDeltas); + + EXPECT_NEAR(12.02, twist.dx.value(), 0.1); + EXPECT_NEAR(-7.07, twist.dy.value(), 0.1); + EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1); +} + TEST_F(MecanumDriveKinematicsTest, Desaturate) { MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps}; wheelSpeeds.Desaturate(5.5_mps); diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp index f0c3092c91..253007f0fd 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp @@ -2,7 +2,11 @@ // 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/MecanumDriveOdometry.h" +#include "frc/trajectory/TrajectoryGenerator.h" #include "gtest/gtest.h" using namespace frc; @@ -14,17 +18,19 @@ class MecanumDriveOdometryTest : public ::testing::Test { Translation2d m_bl{-12_m, 12_m}; Translation2d m_br{-12_m, -12_m}; + MecanumDriveWheelPositions zero; + MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br}; - MecanumDriveOdometry odometry{kinematics, 0_rad}; + MecanumDriveOdometry odometry{kinematics, 0_rad, zero}; }; TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) { - odometry.ResetPosition(Pose2d{}, 0_rad); - MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps, - 3.536_mps}; + MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m}; - odometry.UpdateWithTime(0_s, 0_deg, wheelSpeeds); - auto secondPose = odometry.UpdateWithTime(0.0_s, 0_deg, wheelSpeeds); + odometry.ResetPosition(Pose2d{}, 0_rad, wheelDeltas); + + odometry.Update(0_deg, wheelDeltas); + auto secondPose = odometry.Update(0_deg, wheelDeltas); EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01); EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01); @@ -32,11 +38,12 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) { } TEST_F(MecanumDriveOdometryTest, TwoIterations) { - odometry.ResetPosition(Pose2d{}, 0_rad); - MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps}; + odometry.ResetPosition(Pose2d{}, 0_rad, zero); + MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m, + 0.3536_m}; - odometry.UpdateWithTime(0_s, 0_deg, MecanumDriveWheelSpeeds{}); - auto pose = odometry.UpdateWithTime(0.10_s, 0_deg, speeds); + odometry.Update(0_deg, MecanumDriveWheelPositions{}); + auto pose = odometry.Update(0_deg, wheelDeltas); EXPECT_NEAR(pose.X().value(), 0.3536, 0.01); EXPECT_NEAR(pose.Y().value(), 0.0, 0.01); @@ -44,11 +51,11 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) { } TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) { - odometry.ResetPosition(Pose2d{}, 0_rad); - MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps, - 39.986_mps}; - odometry.UpdateWithTime(0_s, 0_deg, MecanumDriveWheelSpeeds{}); - auto pose = odometry.UpdateWithTime(1_s, 90_deg, speeds); + odometry.ResetPosition(Pose2d{}, 0_rad, zero); + MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m, + 39.986_m}; + odometry.Update(0_deg, MecanumDriveWheelPositions{}); + auto pose = odometry.Update(90_deg, wheelDeltas); EXPECT_NEAR(pose.X().value(), 8.4855, 0.01); EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01); @@ -56,14 +63,140 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) { } TEST_F(MecanumDriveOdometryTest, GyroAngleReset) { - odometry.ResetPosition(Pose2d{}, 90_deg); + odometry.ResetPosition(Pose2d{}, 90_deg, zero); - MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps}; + MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m, + 0.3536_m}; - odometry.UpdateWithTime(0_s, 90_deg, MecanumDriveWheelSpeeds{}); - auto pose = odometry.UpdateWithTime(0.10_s, 90_deg, speeds); + odometry.Update(90_deg, MecanumDriveWheelPositions{}); + auto pose = odometry.Update(90_deg, wheelDeltas); EXPECT_NEAR(pose.X().value(), 0.3536, 0.01); EXPECT_NEAR(pose.Y().value(), 0.0, 0.01); EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01); } + +TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) { + frc::MecanumDriveKinematics 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::MecanumDriveWheelPositions wheelPositions; + + frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{}, + wheelPositions, frc::Pose2d{}}; + + 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; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + auto wheelSpeeds = kinematics.ToWheelSpeeds( + {groundTruthState.velocity, 0_mps, + groundTruthState.velocity * groundTruthState.curvature}); + + wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps; + wheelSpeeds.frontRight += distribution(generator) * 0.1_mps; + wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps; + wheelSpeeds.rearRight += distribution(generator) * 0.1_mps; + + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; + + auto xhat = + odometry.Update(groundTruthState.pose.Rotation() + + frc::Rotation2d{distribution(generator) * 0.05_rad}, + wheelPositions); + 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); +} + +TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) { + frc::MecanumDriveKinematics 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::MecanumDriveWheelPositions wheelPositions; + + frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{}, + wheelPositions, frc::Pose2d{}}; + + 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; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + auto wheelSpeeds = kinematics.ToWheelSpeeds( + {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(), + groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(), + 0_rad_per_s}); + + wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps; + wheelSpeeds.frontRight += distribution(generator) * 0.1_mps; + wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps; + wheelSpeeds.rearRight += distribution(generator) * 0.1_mps; + + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; + + auto xhat = odometry.Update( + frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions); + 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); +}