diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index 7918f9fbb8e..c235420eace 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -27,6 +27,7 @@ *

{@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as * you want; if you never call it then this class will behave exactly like regular encoder odometry. */ +@SuppressWarnings("PMD.UnusedFormalParameter") public class DifferentialDrivePoseEstimator extends PoseEstimator { /** * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and @@ -82,7 +83,6 @@ public DifferentialDrivePoseEstimator( Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { super( - kinematics, new DifferentialDriveOdometry( gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), stateStdDevs, diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index 59bbb3235cd..d4cbd66d174 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 @@ -75,7 +75,6 @@ public MecanumDrivePoseEstimator( Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { super( - kinematics, new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters), stateStdDevs, visionMeasurementStdDevs); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 1d9a54b931a..fd187a30d3b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -10,17 +10,15 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; -import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.WheelPositions; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import java.util.Map; import java.util.NoSuchElementException; -import java.util.Objects; +import java.util.Optional; /** * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder @@ -36,19 +34,19 @@ * never call it then this class will behave exactly like regular encoder odometry. */ public class PoseEstimator> { - private final Kinematics m_kinematics; private final Odometry m_odometry; private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); private final Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); private static final double kBufferDuration = 1.5; - private final TimeInterpolatableBuffer m_poseBuffer = + private final TimeInterpolatableBuffer m_poseBuffer = TimeInterpolatableBuffer.createBuffer(kBufferDuration); + private Pose2d m_poseEstimate; + /** * Constructs a PoseEstimator. * - * @param kinematics A correctly-configured kinematics object for your drivetrain. * @param odometry A correctly-configured odometry object for your drivetrain. * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position * in meters, and heading in radians). Increase these numbers to trust your state estimate @@ -58,13 +56,11 @@ public class PoseEstimator> { * the vision pose measurement less. */ public PoseEstimator( - Kinematics kinematics, - Odometry odometry, - Matrix stateStdDevs, - Matrix visionMeasurementStdDevs) { - m_kinematics = kinematics; + Odometry odometry, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { m_odometry = odometry; + m_poseEstimate = odometry.getPoseMeters(); + for (int i = 0; i < 3; ++i) { m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); } @@ -112,6 +108,7 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet // Reset state estimate and error covariance m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters); m_poseBuffer.clear(); + m_poseEstimate = m_odometry.getPoseMeters(); } /** @@ -120,7 +117,19 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet * @return The estimated robot pose in meters. */ public Pose2d getEstimatedPosition() { - return m_odometry.getPoseMeters(); + return m_poseEstimate; + } + + /** + * Return the pose at a given timestamp, if one exists. + * + * @param timestamp The pose's timestamp in seconds. + * @return The pose at a given timestamp, if one exists. + */ + public Optional sampleAt(double timestamp) { + return m_poseBuffer + .getSample(timestamp) + .map(sample -> sample.plus(new Transform2d(m_odometry.getPoseMeters(), m_poseEstimate))); } /** @@ -160,35 +169,27 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS return; } - // Step 2: Measure the twist between the odometry pose and the vision pose. - var twist = sample.get().poseMeters.log(visionRobotPoseMeters); + // Step 2: Record the odometry updates that have occurred since the vision measurement + var odometry_backtrack = new Transform2d(m_odometry.getPoseMeters(), sample.get()); + var odometry_forward = new Transform2d(sample.get(), m_odometry.getPoseMeters()); + + // Step 3: Revert said odometry updates to get a state estimate from when the vision + // measurement occurred. + var old_estimate = m_poseEstimate.plus(odometry_backtrack); - // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // Step 4: Measure the twist between the odometry pose and the vision pose. + var twist = old_estimate.log(visionRobotPoseMeters); + + // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman // gain matrix representing how much we trust vision measurements compared to our current pose. var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta)); - // Step 4: Convert back to Twist2d. + // Step 6: Convert back to Twist2d. var scaledTwist = new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0)); - // Step 5: Reset Odometry to state at sample with vision adjustment. - m_odometry.resetPosition( - sample.get().gyroAngle, - sample.get().wheelPositions, - sample.get().poseMeters.exp(scaledTwist)); - - // Step 6: Record the current pose to allow multiple measurements from the same timestamp - m_poseBuffer.addSample( - timestampSeconds, - new InterpolationRecord( - getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions)); - - // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the - // pose buffer and correct odometry. - for (Map.Entry entry : - m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) { - updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions); - } + // Step 7: Apply this adjustment to the old estimate, then replay odometry updates. + m_poseEstimate = old_estimate.exp(scaledTwist).plus(odometry_forward); } /** @@ -247,87 +248,12 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { * @return The estimated pose of the robot in meters. */ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) { - m_odometry.update(gyroAngle, wheelPositions); - m_poseBuffer.addSample( - currentTimeSeconds, - new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy())); + var lastOdom = m_odometry.getPoseMeters(); + var currOdom = m_odometry.update(gyroAngle, wheelPositions); + m_poseBuffer.addSample(currentTimeSeconds, currOdom); - return getEstimatedPosition(); - } - - /** - * Represents an odometry record. The record contains the inputs provided as well as the pose that - * was observed based on these inputs, as well as the previous record and its inputs. - */ - private class InterpolationRecord implements Interpolatable { - // The pose observed given the current sensor inputs and the previous pose. - private final Pose2d poseMeters; + m_poseEstimate = m_poseEstimate.exp(lastOdom.log(currOdom)); - // The current gyro angle. - private final Rotation2d gyroAngle; - - // The current encoder readings. - private final T wheelPositions; - - /** - * Constructs an Interpolation Record with the specified parameters. - * - * @param poseMeters The pose observed given the current sensor inputs and the previous pose. - * @param gyro The current gyro angle. - * @param wheelPositions The current encoder readings. - */ - private InterpolationRecord(Pose2d poseMeters, Rotation2d gyro, T wheelPositions) { - this.poseMeters = poseMeters; - this.gyroAngle = gyro; - this.wheelPositions = wheelPositions; - } - - /** - * Return the interpolated record. This object is assumed to be the starting position, or lower - * bound. - * - * @param endValue The upper bound, or end. - * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1]. - * @return The interpolated value. - */ - @Override - public InterpolationRecord interpolate(InterpolationRecord endValue, double t) { - if (t < 0) { - return this; - } else if (t >= 1) { - return endValue; - } else { - // Find the new wheel distances. - var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t); - - // Find the new gyro angle. - var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t); - - // Create a twist to represent the change based on the interpolated sensor inputs. - Twist2d twist = m_kinematics.toTwist2d(wheelPositions, wheelLerp); - twist.dtheta = gyroLerp.minus(gyroAngle).getRadians(); - - return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp); - } - } - - @Override - public boolean equals(Object obj) { - if (this == obj) { - return true; - } - if (!(obj instanceof PoseEstimator.InterpolationRecord)) { - return false; - } - var record = (PoseEstimator.InterpolationRecord) obj; - return Objects.equals(gyroAngle, record.gyroAngle) - && Objects.equals(wheelPositions, record.wheelPositions) - && Objects.equals(poseMeters, record.poseMeters); - } - - @Override - public int hashCode() { - return Objects.hash(gyroAngle, wheelPositions, poseMeters); - } + return getEstimatedPosition(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index f778b621e32..ae406b162cb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -77,7 +77,6 @@ public SwerveDrivePoseEstimator( Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { super( - kinematics, new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters), stateStdDevs, visionMeasurementStdDevs); diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 213ba991760..69e08360aec 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -21,5 +21,5 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( const wpi::array& visionMeasurementStdDevs) : PoseEstimator( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 7c75e71ffee..71da428440b 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -25,5 +25,5 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) : PoseEstimator( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index eb437a40f7e..abfc35ce7b2 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -4,14 +4,16 @@ #pragma once +#include + #include #include #include #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Twist2d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" -#include "frc/kinematics/Kinematics.h" #include "frc/kinematics/Odometry.h" #include "frc/kinematics/WheelPositions.h" #include "units/time.h" @@ -37,8 +39,6 @@ class WPILIB_DLLEXPORT PoseEstimator { /** * Constructs a PoseEstimator. * - * @param kinematics A correctly-configured kinematics object for your - * drivetrain. * @param odometry A correctly-configured odometry object for your drivetrain. * @param stateStdDevs Standard deviations of the pose estimate (x position in * meters, y position in meters, and heading in radians). Increase these @@ -48,8 +48,7 @@ class WPILIB_DLLEXPORT PoseEstimator { * radians). Increase these numbers to trust the vision pose measurement * less. */ - PoseEstimator(Kinematics& kinematics, - Odometry& odometry, + PoseEstimator(Odometry& odometry, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs); @@ -86,6 +85,14 @@ class WPILIB_DLLEXPORT PoseEstimator { */ Pose2d GetEstimatedPosition() const; + /** + * Return the pose at a given timestamp, if one exists. + * + * @param timestamp The pose's timestamp. + * @return The pose at a given timestamp, if one exists. + */ + std::optional SampleAt(units::second_t timestamp) const; + /** * Adds a vision measurement to the Kalman Filter. This will correct * the odometry pose estimate while still accounting for measurement noise. @@ -171,57 +178,25 @@ class WPILIB_DLLEXPORT PoseEstimator { const WheelPositions& wheelPositions); private: - struct InterpolationRecord { - // The pose observed given the current sensor inputs and the previous pose. - Pose2d pose; - - // The current gyroscope angle. - Rotation2d gyroAngle; - - // The distances traveled by the wheels. - WheelPositions wheelPositions; - - /** - * Checks equality between this InterpolationRecord and another object. - * - * @param other The other object. - * @return Whether the two objects are equal. - */ - bool operator==(const InterpolationRecord& other) const = default; - - /** - * Checks inequality between this InterpolationRecord and another object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const InterpolationRecord& other) const = default; - - /** - * Interpolates between two InterpolationRecords. - * - * @param endValue The end value for the interpolation. - * @param i The interpolant (fraction). - * - * @return The interpolated state. - */ - InterpolationRecord Interpolate( - Kinematics& kinematics, - InterpolationRecord endValue, double i) const; - }; - static constexpr units::second_t kBufferDuration = 1.5_s; - Kinematics& m_kinematics; Odometry& m_odometry; wpi::array m_q{wpi::empty_array}; Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); - TimeInterpolatableBuffer m_poseBuffer{ - kBufferDuration, [this](const InterpolationRecord& start, - const InterpolationRecord& end, double t) { - return start.Interpolate(this->m_kinematics, end, t); + TimeInterpolatableBuffer m_poseBuffer{ + kBufferDuration, [](const Pose2d& start, const Pose2d& end, double t) { + if (t <= 0) { + return start; + } else if (t >= 1) { + return end; + } else { + auto twist = start.Log(end); + Twist2d scaledTwist{twist.dx * t, twist.dy * t, twist.dtheta * t}; + return start.Exp(scaledTwist); + } }}; + Pose2d m_poseEstimate; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc index 79d71bcc566..aa6e7ea79d1 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc @@ -10,11 +10,10 @@ namespace frc { template PoseEstimator::PoseEstimator( - Kinematics& kinematics, Odometry& odometry, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : m_kinematics(kinematics), m_odometry(odometry) { + : m_odometry(odometry), m_poseEstimate{odometry.GetPose()} { for (size_t i = 0; i < 3; ++i) { m_q[i] = stateStdDevs[i] * stateStdDevs[i]; } @@ -49,12 +48,25 @@ void PoseEstimator::ResetPosition( // Reset state estimate and error covariance m_odometry.ResetPosition(gyroAngle, wheelPositions, pose); m_poseBuffer.Clear(); + m_poseEstimate = m_odometry.GetPose(); } template Pose2d PoseEstimator::GetEstimatedPosition() const { - return m_odometry.GetPose(); + return m_poseEstimate; +} + +template +std::optional PoseEstimator::SampleAt( + units::second_t timestamp) const { + // TODO Use C++23 std::optional::transform + std::optional sample = m_poseBuffer.Sample(timestamp); + if (sample) { + return *sample + Transform2d{m_odometry.GetPose(), m_poseEstimate}; + } else { + return std::nullopt; + } } template @@ -75,44 +87,33 @@ void PoseEstimator::AddVisionMeasurement( return; } - // Step 2: Measure the twist between the odometry pose and the vision pose - auto twist = sample.value().pose.Log(visionRobotPose); + // Step 2: Record the odometry updates that have occurred since the vision + // measurement + Transform2d odometry_backtrack{m_odometry.GetPose(), *sample}; + Transform2d odometry_forward{*sample, m_odometry.GetPose()}; + + // Step 3: Revert said odometry updates to get a state estimate from when the + // vision measurement occurred. + auto old_estimate = m_poseEstimate + odometry_backtrack; - // Step 3: We should not trust the twist entirely, so instead we scale this + // Step 4: Measure the twist between the odometry pose and the vision pose. + auto twist = old_estimate.Log(visionRobotPose); + + // Step 5: We should not trust the twist entirely, so instead we scale this // twist by a Kalman gain matrix representing how much we trust vision // measurements compared to our current pose. Eigen::Vector3d k_times_twist = m_visionK * Eigen::Vector3d{twist.dx.value(), twist.dy.value(), twist.dtheta.value()}; - // Step 4: Convert back to Twist2d + // Step 6: Convert back to Twist2d. Twist2d scaledTwist{units::meter_t{k_times_twist(0)}, units::meter_t{k_times_twist(1)}, units::radian_t{k_times_twist(2)}}; - // Step 5: Reset Odometry to state at sample with vision adjustment. - m_odometry.ResetPosition(sample.value().gyroAngle, - sample.value().wheelPositions, - sample.value().pose.Exp(scaledTwist)); - - // Step 6: Record the current pose to allow multiple measurements from the - // same timestamp - m_poseBuffer.AddSample(timestamp, - {GetEstimatedPosition(), sample.value().gyroAngle, - sample.value().wheelPositions}); - - // Step 7: Replay odometry inputs between sample time and latest recorded - // sample to update the pose buffer and correct odometry. - auto internal_buf = m_poseBuffer.GetInternalBuffer(); - - auto upper_bound = - std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp, - [](const auto& pair, auto t) { return t > pair.first; }); - - for (auto entry = upper_bound; entry != internal_buf.end(); entry++) { - UpdateWithTime(entry->first, entry->second.gyroAngle, - entry->second.wheelPositions); - } + // Step 7: Apply this adjustment to the old estimate, then replay dometry + // updates. + m_poseEstimate = old_estimate.Exp(scaledTwist) + odometry_forward; } template @@ -126,40 +127,13 @@ template Pose2d PoseEstimator::UpdateWithTime( units::second_t currentTime, const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { - m_odometry.Update(gyroAngle, wheelPositions); - - WheelPositions internalWheelPositions = wheelPositions; + auto lastOdom = m_odometry.GetPose(); + auto currOdom = m_odometry.Update(gyroAngle, wheelPositions); + m_poseBuffer.AddSample(currentTime, currOdom); - m_poseBuffer.AddSample( - currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions}); + m_poseEstimate = m_poseEstimate.Exp(lastOdom.Log(currOdom)); return GetEstimatedPosition(); } -template -typename PoseEstimator::InterpolationRecord -PoseEstimator::InterpolationRecord::Interpolate( - Kinematics& kinematics, - InterpolationRecord endValue, double i) const { - if (i < 0) { - return *this; - } else if (i > 1) { - return endValue; - } else { - // Find the new wheel distance measurements. - WheelPositions wheels_lerp = - this->wheelPositions.Interpolate(endValue.wheelPositions, i); - - // Find the new gyro angle. - auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i); - - // Create a twist to represent the change based on the interpolated - // sensor inputs. - auto twist = kinematics.ToTwist2d(this->wheelPositions, wheels_lerp); - twist.dtheta = (gyro - gyroAngle).Radians(); - - return {pose.Exp(twist), gyro, wheels_lerp}; - } -} - } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 43831d7aff4..a43ee45a769 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -85,7 +85,7 @@ class SwerveDrivePoseEstimator const wpi::array& visionMeasurementStdDevs) : PoseEstimator, SwerveDriveWheelPositions>( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} /** diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h index 94d28180d8f..bfb1b548976 100644 --- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h +++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h @@ -100,7 +100,7 @@ class TimeInterpolatableBuffer { * * @param time The time at which to sample the buffer. */ - std::optional Sample(units::second_t time) { + std::optional Sample(units::second_t time) const { if (m_pastSnapshots.empty()) { return {}; } 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 deleted file mode 100644 index ea08a5f1d90..00000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java +++ /dev/null @@ -1,324 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.estimator; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; - -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -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.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import java.util.List; -import java.util.Random; -import java.util.TreeMap; -import java.util.function.Function; -import org.junit.jupiter.api.Test; - -class MecanumDrivePoseEstimatorTest { - @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 estimator = - new MecanumDrivePoseEstimator( - kinematics, - new Rotation2d(), - wheelPositions, - new Pose2d(), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.45, 0.45, 0.1)); - - var trajectory = - TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), - new Pose2d(0, 0, Rotation2d.fromDegrees(135)), - new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), - new Pose2d(0, 0, Rotation2d.fromDegrees(45))), - new TrajectoryConfig(2, 2)); - - testFollowTrajectory( - kinematics, - estimator, - trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, - trajectory.getInitialPose(), - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - 0.02, - 0.1, - 0.25, - true); - } - - @Test - void testBadInitialPose() { - 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( - kinematics, - new Rotation2d(), - wheelPositions, - new Pose2d(), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.45, 0.45, 0.1)); - - var trajectory = - TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), - new Pose2d(0, 0, Rotation2d.fromDegrees(135)), - new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), - new Pose2d(0, 0, Rotation2d.fromDegrees(45))), - new TrajectoryConfig(2, 2)); - - for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) { - for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) { - var pose_offset = Rotation2d.fromDegrees(offset_direction_degs); - var heading_offset = Rotation2d.fromDegrees(offset_heading_degs); - - var initial_pose = - trajectory - .getInitialPose() - .plus( - new Transform2d( - new Translation2d(pose_offset.getCos(), pose_offset.getSin()), - heading_offset)); - - testFollowTrajectory( - kinematics, - estimator, - trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, - initial_pose, - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - 0.02, - 0.1, - 0.25, - false); - } - } - } - - void testFollowTrajectory( - final MecanumDriveKinematics kinematics, - final MecanumDrivePoseEstimator estimator, - final Trajectory trajectory, - final Function chassisSpeedsGenerator, - final Function visionMeasurementGenerator, - final Pose2d startingPose, - final Pose2d endingPose, - final double dt, - final double visionUpdateRate, - final double visionUpdateDelay, - final boolean checkError) { - var wheelPositions = new MecanumDriveWheelPositions(); - - estimator.resetPosition(new Rotation2d(), wheelPositions, startingPose); - - var rand = new Random(3538); - - double t = 0.0; - - final TreeMap visionUpdateQueue = new TreeMap<>(); - - double maxError = Double.NEGATIVE_INFINITY; - double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { - var groundTruthState = trajectory.sample(t); - - // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the - // last vision measurement - if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) { - Pose2d newVisionPose = - visionMeasurementGenerator - .apply(groundTruthState) - .plus( - new Transform2d( - new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1), - new Rotation2d(rand.nextGaussian() * 0.05))); - - visionUpdateQueue.put(t, newVisionPose); - } - - // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds - // since it was measured - if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { - var visionEntry = visionUpdateQueue.pollFirstEntry(); - estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey()); - } - - var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); - - var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); - - 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, - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)) - .minus(trajectory.getInitialPose().getRotation()), - wheelPositions); - - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); - if (error > maxError) { - maxError = error; - } - errorSum += error; - - t += dt; - } - - assertEquals( - endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X"); - assertEquals( - endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); - assertEquals( - endingPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), - 0.15, - "Incorrect Final Theta"); - - if (checkError) { - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); - assertEquals(0.0, maxError, 0.2, "Incorrect max error"); - } - } - - @Test - void testSimultaneousVisionMeasurements() { - // This tests for multiple vision measurements appled at the same time. The expected behavior - // is that all measurements affect the estimated pose. The alternative result is that only one - // vision measurement affects the outcome. If that were the case, after 1000 measurements, the - // estimated pose would converge to that measurement. - 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( - kinematics, - new Rotation2d(), - wheelPositions, - new Pose2d(1, 2, Rotation2d.fromDegrees(270)), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.45, 0.45, 0.1)); - - estimator.updateWithTime(0, new Rotation2d(), wheelPositions); - - var visionMeasurements = - new Pose2d[] { - new Pose2d(0, 0, Rotation2d.fromDegrees(0)), - new Pose2d(3, 1, Rotation2d.fromDegrees(90)), - new Pose2d(2, 4, Rotation2d.fromRadians(180)), - }; - - for (int i = 0; i < 1000; i++) { - for (var measurement : visionMeasurements) { - estimator.addVisionMeasurement(measurement, 0); - } - } - - for (var measurement : visionMeasurements) { - var errorLog = - "Estimator converged to one vision measurement: " - + estimator.getEstimatedPosition().toString() - + " -> " - + measurement; - - var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX()); - var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY()); - var dtheta = - Math.abs( - measurement.getRotation().getDegrees() - - estimator.getEstimatedPosition().getRotation().getDegrees()); - - assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog); - } - } - - @Test - void testDiscardsOldVisionMeasurements() { - var kinematics = - new MecanumDriveKinematics( - new Translation2d(1, 1), - new Translation2d(-1, 1), - new Translation2d(1, -1), - new Translation2d(-1, -1)); - var estimator = - new MecanumDrivePoseEstimator( - kinematics, - new Rotation2d(), - new MecanumDriveWheelPositions(), - new Pose2d(), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); - - double time = 0; - - // Add enough measurements to fill up the buffer - for (; time < 4; time += 0.02) { - estimator.updateWithTime(time, new Rotation2d(), new MecanumDriveWheelPositions()); - } - - var odometryPose = estimator.getEstimatedPosition(); - - // Apply a vision measurement made 3 seconds ago - // This test passes if this does not cause a ConcurrentModificationException. - estimator.addVisionMeasurement( - new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)), - 1, - VecBuilder.fill(0.1, 0.1, 0.1)); - - assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"); - assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"); - assertEquals( - odometryPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), - "Incorrect Final Theta"); - } -} diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimationTestUtil.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimationTestUtil.java new file mode 100644 index 00000000000..8458f8f2a09 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimationTestUtil.java @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Kinematics; +import edu.wpi.first.math.kinematics.WheelPositions; + +public class PoseEstimationTestUtil { + /** + * This class is intended for use as "drivetrain-agnostic" kinematics. Odometry measurements are + * akin to Poses, while their difference is akin to Twists. + */ + public static class SE2KinematicPrimitive implements WheelPositions { + private final Pose2d pose; + + public SE2KinematicPrimitive(final Pose2d pose) { + this.pose = pose; + } + + @Override + public SE2KinematicPrimitive copy() { + return this; + } + + @Override + public SE2KinematicPrimitive interpolate(SE2KinematicPrimitive end, double t) { + var twist = this.pose.log(end.pose); + var scaled = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t); + + return new SE2KinematicPrimitive(this.pose.exp(scaled)); + } + } + + public static class SE2Kinematics + implements Kinematics { + private final double timeConstant; + + public SE2Kinematics(double timeConstant) { + this.timeConstant = timeConstant; + } + + @Override + public ChassisSpeeds toChassisSpeeds(SE2KinematicPrimitive wheelSpeeds) { + var twist = new Pose2d().log(wheelSpeeds.pose); + + return new ChassisSpeeds( + twist.dx * timeConstant, twist.dy * timeConstant, twist.dtheta * timeConstant); + } + + @Override + public SE2KinematicPrimitive toWheelSpeeds(ChassisSpeeds chassisSpeeds) { + return new SE2KinematicPrimitive( + new Pose2d() + .exp( + new Twist2d( + chassisSpeeds.vxMetersPerSecond / timeConstant, + chassisSpeeds.vyMetersPerSecond / timeConstant, + chassisSpeeds.omegaRadiansPerSecond / timeConstant))); + } + + @Override + public Twist2d toTwist2d(SE2KinematicPrimitive start, SE2KinematicPrimitive end) { + return start.pose.log(end.pose); + } + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimatorTest.java similarity index 64% rename from wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java rename to wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimatorTest.java index 08c5a156cf1..4e04ddd3225 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimatorTest.java @@ -8,35 +8,44 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.estimator.PoseEstimationTestUtil.SE2KinematicPrimitive; +import edu.wpi.first.math.estimator.PoseEstimationTestUtil.SE2Kinematics; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.Odometry; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.util.SamplingUtil; import java.util.List; import java.util.Random; import java.util.TreeMap; -import java.util.function.Function; import org.junit.jupiter.api.Test; -class DifferentialDrivePoseEstimatorTest { +class PoseEstimatorTest { + private final Vector stateStdDevs = VecBuilder.fill(0.05, 0.05, 0.05); + private final Vector stateNoiseStdDevs = stateStdDevs.div(50); + private final Vector visionStdDevs = VecBuilder.fill(0.2, 0.2, 0.2); + + private static final double visionUpdatePeriod = 0.2; + private static final double visionUpdateDelay = 0.1; + + private static final double dt = 0.02; + @Test void testAccuracy() { - var kinematics = new DifferentialDriveKinematics(1); + var kinematics = new SE2Kinematics(dt); + + var odometry = + new Odometry<>( + kinematics, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()), new Pose2d()); + + var estimator = new PoseEstimator<>(odometry, stateStdDevs, visionStdDevs); - var estimator = - new DifferentialDrivePoseEstimator( - kinematics, - new Rotation2d(), - 0, - 0, - new Pose2d(), - VecBuilder.fill(0.02, 0.02, 0.01), - VecBuilder.fill(0.1, 0.1, 0.1)); var trajectory = TrajectoryGenerator.generateTrajectory( List.of( @@ -51,33 +60,25 @@ void testAccuracy() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, trajectory.getInitialPose(), - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - 0.02, - 0.1, - 0.25, - true); + trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters, + dt, + visionUpdatePeriod, + visionUpdateDelay, + true, + stateNoiseStdDevs, + visionStdDevs); } @Test void testBadInitialPose() { - var kinematics = new DifferentialDriveKinematics(1); + var kinematics = new SE2Kinematics(dt); - var estimator = - new DifferentialDrivePoseEstimator( - kinematics, - new Rotation2d(), - 0, - 0, - new Pose2d(), - VecBuilder.fill(0.02, 0.02, 0.01), - VecBuilder.fill(0.1, 0.1, 0.1)); + var odometry = + new Odometry<>( + kinematics, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()), new Pose2d()); + + var estimator = new PoseEstimator<>(odometry, stateStdDevs, visionStdDevs); var trajectory = TrajectoryGenerator.generateTrajectory( @@ -106,39 +107,34 @@ void testBadInitialPose() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, initial_pose, - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - 0.02, - 0.1, - 0.25, - false); + trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters, + dt, + visionUpdatePeriod, + visionUpdateDelay, + false, + stateNoiseStdDevs, + visionStdDevs); } } } void testFollowTrajectory( - final DifferentialDriveKinematics kinematics, - final DifferentialDrivePoseEstimator estimator, + final SE2Kinematics kinematics, + final PoseEstimator estimator, final Trajectory trajectory, - final Function chassisSpeedsGenerator, - final Function visionMeasurementGenerator, final Pose2d startingPose, final Pose2d endingPose, final double dt, final double visionUpdateRate, final double visionUpdateDelay, - final boolean checkError) { - double leftDistanceMeters = 0; - double rightDistanceMeters = 0; - + final boolean checkError, + final Vector stateStdDevs, + final Vector visionStdDevs) { estimator.resetPosition( - new Rotation2d(), leftDistanceMeters, rightDistanceMeters, startingPose); + trajectory.getInitialPose().getRotation(), + new SE2KinematicPrimitive(trajectory.getInitialPose()), + startingPose); var rand = new Random(3538); @@ -154,13 +150,8 @@ void testFollowTrajectory( // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the // last vision measurement if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) { - Pose2d newVisionPose = - visionMeasurementGenerator - .apply(groundTruthState) - .plus( - new Transform2d( - new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1), - new Rotation2d(rand.nextGaussian() * 0.05))); + var adjustment = SamplingUtil.sampleTwist2d(rand, visionStdDevs); + Pose2d newVisionPose = groundTruthState.poseMeters.exp(adjustment); visionUpdateQueue.put(t, newVisionPose); } @@ -169,26 +160,21 @@ void testFollowTrajectory( // since it was measured if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { var visionEntry = visionUpdateQueue.pollFirstEntry(); + estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey()); } - var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); + var gyroAngle = + groundTruthState + .poseMeters + .getRotation() + .plus(SamplingUtil.sampleRotation2d(rand, stateStdDevs.get(2, 0))); - var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + var update = + new SE2KinematicPrimitive( + groundTruthState.poseMeters.exp(SamplingUtil.sampleTwist2d(rand, stateStdDevs))); - leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt; - rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt; - - var xHat = - estimator.updateWithTime( - t, - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)) - .minus(trajectory.getInitialPose().getRotation()), - leftDistanceMeters, - rightDistanceMeters); + var xHat = estimator.updateWithTime(t, gyroAngle, update); double error = groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); @@ -201,9 +187,9 @@ void testFollowTrajectory( } assertEquals( - endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X"); + endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.15, "Incorrect Final X"); assertEquals( - endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); + endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.15, "Incorrect Final Y"); assertEquals( endingPose.getRotation().getRadians(), estimator.getEstimatedPosition().getRotation().getRadians(), @@ -212,8 +198,8 @@ void testFollowTrajectory( if (checkError) { assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); - assertEquals(0.0, maxError, 0.2, "Incorrect max error"); + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.3, "Incorrect max error"); } } @@ -223,19 +209,13 @@ void testSimultaneousVisionMeasurements() { // is that all measurements affect the estimated pose. The alternative result is that only one // vision measurement affects the outcome. If that were the case, after 1000 measurements, the // estimated pose would converge to that measurement. - var kinematics = new DifferentialDriveKinematics(1); - - var estimator = - new DifferentialDrivePoseEstimator( - kinematics, - new Rotation2d(), - 0, - 0, - new Pose2d(1, 2, Rotation2d.fromDegrees(270)), - VecBuilder.fill(0.02, 0.02, 0.01), - VecBuilder.fill(0.1, 0.1, 0.1)); + var kinematics = new SE2Kinematics(dt); + var odometry = + new Odometry<>( + kinematics, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()), new Pose2d()); + var estimator = new PoseEstimator<>(odometry, stateStdDevs, visionStdDevs); - estimator.updateWithTime(0, new Rotation2d(), 0, 0); + estimator.updateWithTime(0, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d())); var visionMeasurements = new Pose2d[] { @@ -270,22 +250,17 @@ void testSimultaneousVisionMeasurements() { @Test void testDiscardsStaleVisionMeasurements() { - var kinematics = new DifferentialDriveKinematics(1); - var estimator = - new DifferentialDrivePoseEstimator( - kinematics, - new Rotation2d(), - 0, - 0, - new Pose2d(), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); + var kinematics = new SE2Kinematics(dt); + var odometry = + new Odometry<>( + kinematics, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()), new Pose2d()); + var estimator = new PoseEstimator<>(odometry, stateStdDevs, visionStdDevs); double time = 0; // Add enough measurements to fill up the buffer for (; time < 4; time += 0.02) { - estimator.updateWithTime(time, new Rotation2d(), 0, 0); + estimator.updateWithTime(time, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d())); } var odometryPose = estimator.getEstimatedPosition(); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java deleted file mode 100644 index f827a102527..00000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ /dev/null @@ -1,358 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.estimator; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; - -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import java.util.List; -import java.util.Random; -import java.util.TreeMap; -import java.util.function.Function; -import org.junit.jupiter.api.Test; - -class SwerveDrivePoseEstimatorTest { - @Test - void testAccuracyFacingTrajectory() { - var kinematics = - new SwerveDriveKinematics( - new Translation2d(1, 1), - new Translation2d(1, -1), - new Translation2d(-1, -1), - new Translation2d(-1, 1)); - - var fl = new SwerveModulePosition(); - var fr = new SwerveModulePosition(); - var bl = new SwerveModulePosition(); - var br = new SwerveModulePosition(); - - var estimator = - new SwerveDrivePoseEstimator( - kinematics, - new Rotation2d(), - new SwerveModulePosition[] {fl, fr, bl, br}, - new Pose2d(), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.5, 0.5, 0.5)); - - var trajectory = - TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), - new Pose2d(0, 0, Rotation2d.fromDegrees(135)), - new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), - new Pose2d(0, 0, Rotation2d.fromDegrees(45))), - new TrajectoryConfig(2, 2)); - - testFollowTrajectory( - kinematics, - estimator, - trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, - trajectory.getInitialPose(), - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - 0.02, - 0.1, - 0.25, - true); - } - - @Test - void testBadInitialPose() { - var kinematics = - new SwerveDriveKinematics( - new Translation2d(1, 1), - new Translation2d(1, -1), - new Translation2d(-1, -1), - new Translation2d(-1, 1)); - - var fl = new SwerveModulePosition(); - var fr = new SwerveModulePosition(); - var bl = new SwerveModulePosition(); - var br = new SwerveModulePosition(); - - var estimator = - new SwerveDrivePoseEstimator( - kinematics, - new Rotation2d(), - new SwerveModulePosition[] {fl, fr, bl, br}, - new Pose2d(-1, -1, Rotation2d.fromRadians(-1)), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); - var trajectory = - TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - new Pose2d(3, 0, Rotation2d.fromDegrees(-90)), - new Pose2d(0, 0, Rotation2d.fromDegrees(135)), - new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)), - new Pose2d(0, 0, Rotation2d.fromDegrees(45))), - new TrajectoryConfig(2, 2)); - - for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) { - for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) { - var pose_offset = Rotation2d.fromDegrees(offset_direction_degs); - var heading_offset = Rotation2d.fromDegrees(offset_heading_degs); - - var initial_pose = - trajectory - .getInitialPose() - .plus( - new Transform2d( - new Translation2d(pose_offset.getCos(), pose_offset.getSin()), - heading_offset)); - - testFollowTrajectory( - kinematics, - estimator, - trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, - initial_pose, - new Pose2d(0, 0, Rotation2d.fromDegrees(45)), - 0.02, - 0.1, - 1.0, - false); - } - } - } - - void testFollowTrajectory( - final SwerveDriveKinematics kinematics, - final SwerveDrivePoseEstimator estimator, - final Trajectory trajectory, - final Function chassisSpeedsGenerator, - final Function visionMeasurementGenerator, - final Pose2d startingPose, - final Pose2d endingPose, - final double dt, - final double visionUpdateRate, - final double visionUpdateDelay, - final boolean checkError) { - SwerveModulePosition[] positions = { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - - estimator.resetPosition(new Rotation2d(), positions, startingPose); - - var rand = new Random(3538); - - double t = 0.0; - - final TreeMap visionUpdateQueue = new TreeMap<>(); - - double maxError = Double.NEGATIVE_INFINITY; - double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { - var groundTruthState = trajectory.sample(t); - - // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the - // last vision measurement - if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) { - Pose2d newVisionPose = - visionMeasurementGenerator - .apply(groundTruthState) - .plus( - new Transform2d( - new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1), - new Rotation2d(rand.nextGaussian() * 0.05))); - - visionUpdateQueue.put(t, newVisionPose); - } - - // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds - // since it was measured - if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { - var visionEntry = visionUpdateQueue.pollFirstEntry(); - estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey()); - } - - var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); - - var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); - - for (int i = 0; i < moduleStates.length; i++) { - positions[i].distanceMeters += - moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt; - positions[i].angle = - moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); - } - - var xHat = - estimator.updateWithTime( - t, - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)) - .minus(trajectory.getInitialPose().getRotation()), - positions); - - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); - if (error > maxError) { - maxError = error; - } - errorSum += error; - - t += dt; - } - - assertEquals( - endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X"); - assertEquals( - endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); - assertEquals( - endingPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), - 0.15, - "Incorrect Final Theta"); - - if (checkError) { - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); - assertEquals(0.0, maxError, 0.2, "Incorrect max error"); - } - } - - @Test - void testSimultaneousVisionMeasurements() { - // This tests for multiple vision measurements appled at the same time. The expected behavior - // is that all measurements affect the estimated pose. The alternative result is that only one - // vision measurement affects the outcome. If that were the case, after 1000 measurements, the - // estimated pose would converge to that measurement. - var kinematics = - new SwerveDriveKinematics( - new Translation2d(1, 1), - new Translation2d(1, -1), - new Translation2d(-1, -1), - new Translation2d(-1, 1)); - - var fl = new SwerveModulePosition(); - var fr = new SwerveModulePosition(); - var bl = new SwerveModulePosition(); - var br = new SwerveModulePosition(); - - var estimator = - new SwerveDrivePoseEstimator( - kinematics, - new Rotation2d(), - new SwerveModulePosition[] {fl, fr, bl, br}, - new Pose2d(1, 2, Rotation2d.fromDegrees(270)), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); - - estimator.updateWithTime(0, new Rotation2d(), new SwerveModulePosition[] {fl, fr, bl, br}); - - var visionMeasurements = - new Pose2d[] { - new Pose2d(0, 0, Rotation2d.fromDegrees(0)), - new Pose2d(3, 1, Rotation2d.fromDegrees(90)), - new Pose2d(2, 4, Rotation2d.fromRadians(180)), - }; - - for (int i = 0; i < 1000; i++) { - for (var measurement : visionMeasurements) { - estimator.addVisionMeasurement(measurement, 0); - } - } - - for (var measurement : visionMeasurements) { - var errorLog = - "Estimator converged to one vision measurement: " - + estimator.getEstimatedPosition().toString() - + " -> " - + measurement; - - var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX()); - var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY()); - var dtheta = - Math.abs( - measurement.getRotation().getDegrees() - - estimator.getEstimatedPosition().getRotation().getDegrees()); - - assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog); - } - } - - @Test - void testDiscardsOldVisionMeasurements() { - var kinematics = - new SwerveDriveKinematics( - new Translation2d(1, 1), - new Translation2d(-1, 1), - new Translation2d(1, -1), - new Translation2d(-1, -1)); - var estimator = - new SwerveDrivePoseEstimator( - kinematics, - new Rotation2d(), - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }, - new Pose2d(), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); - - double time = 0; - - // Add enough measurements to fill up the buffer - for (; time < 4; time += 0.02) { - estimator.updateWithTime( - time, - new Rotation2d(), - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }); - } - - var odometryPose = estimator.getEstimatedPosition(); - - // Apply a vision measurement made 3 seconds ago - // This test passes if this does not cause a ConcurrentModificationException. - estimator.addVisionMeasurement( - new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)), - 1, - VecBuilder.fill(0.1, 0.1, 0.1)); - - assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"); - assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"); - assertEquals( - odometryPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), - "Incorrect Final Theta"); - } -} diff --git a/wpimath/src/test/java/edu/wpi/first/math/util/SamplingUtil.java b/wpimath/src/test/java/edu/wpi/first/math/util/SamplingUtil.java new file mode 100644 index 00000000000..132a73d5452 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/util/SamplingUtil.java @@ -0,0 +1,26 @@ +// 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.util; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.numbers.N3; +import java.util.Random; + +public final class SamplingUtil { + private SamplingUtil() {} + + public static Rotation2d sampleRotation2d(Random rand, double stdev) { + return Rotation2d.fromRadians(rand.nextGaussian() * stdev); + } + + public static Twist2d sampleTwist2d(Random rand, Vector stdev) { + return new Twist2d( + rand.nextGaussian() * stdev.get(0, 0), + rand.nextGaussian() * stdev.get(1, 0), + sampleRotation2d(rand, stdev.get(2, 0)).getRadians()); + } +} diff --git a/wpimath/src/test/native/cpp/SamplingUtil.h b/wpimath/src/test/native/cpp/SamplingUtil.h new file mode 100644 index 00000000000..1fa3fd4857c --- /dev/null +++ b/wpimath/src/test/native/cpp/SamplingUtil.h @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include + +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Twist2d.h" + +namespace frc { + +template +frc::Rotation2d SampleRotation2d(Generator& generator, + std::normal_distribution& distribution, + double stdDev) { + return frc::Rotation2d{units::radian_t{distribution(generator) * stdDev}}; +} + +template +frc::Twist2d SampleTwist2d(Generator& generator, + std::normal_distribution& distribution, + wpi::array stdDev) { + return frc::Twist2d{units::meter_t{distribution(generator) * stdDev[0]}, + units::meter_t{distribution(generator) * stdDev[1]}, + units::radian_t{distribution(generator) * stdDev[2]}}; +} + +} // namespace frc diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp deleted file mode 100644 index 8b3a586892d..00000000000 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ /dev/null @@ -1,297 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include -#include -#include - -#include - -#include "frc/estimator/MecanumDrivePoseEstimator.h" -#include "frc/geometry/Pose2d.h" -#include "frc/kinematics/MecanumDriveKinematics.h" -#include "frc/trajectory/TrajectoryGenerator.h" - -void testFollowTrajectory( - const frc::MecanumDriveKinematics& kinematics, - frc::MecanumDrivePoseEstimator& estimator, - const frc::Trajectory& trajectory, - std::function - chassisSpeedsGenerator, - std::function - visionMeasurementGenerator, - const frc::Pose2d& startingPose, const frc::Pose2d& endingPose, - const units::second_t dt, const units::second_t kVisionUpdateRate, - const units::second_t kVisionUpdateDelay, const bool checkError, - const bool debug) { - frc::MecanumDriveWheelPositions wheelPositions{}; - - estimator.ResetPosition(frc::Rotation2d{}, wheelPositions, startingPose); - - std::default_random_engine generator; - std::normal_distribution distribution(0.0, 1.0); - - units::second_t t = 0_s; - - std::vector> visionPoses; - std::vector> - visionLog; - - double maxError = -std::numeric_limits::max(); - double errorSum = 0; - - if (debug) { - fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n"); - } - - while (t < trajectory.TotalTime()) { - frc::Trajectory::State groundTruthState = trajectory.Sample(t); - - // We are due for a new vision measurement if it's been `visionUpdateRate` - // seconds since the last vision measurement - if (visionPoses.empty() || - visionPoses.back().first + kVisionUpdateRate < t) { - auto visionPose = - visionMeasurementGenerator(groundTruthState) + - frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m, - distribution(generator) * 0.1_m}, - frc::Rotation2d{distribution(generator) * 0.05_rad}}; - visionPoses.push_back({t, visionPose}); - } - - // We should apply the oldest vision measurement if it has been - // `visionUpdateDelay` seconds since it was measured - if (!visionPoses.empty() && - visionPoses.front().first + kVisionUpdateDelay < t) { - auto visionEntry = visionPoses.front(); - estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first); - visionPoses.erase(visionPoses.begin()); - visionLog.push_back({t, visionEntry.first, visionEntry.second}); - } - - auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState); - - auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); - - 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} - - trajectory.InitialPose().Rotation(), - wheelPositions); - - if (debug) { - fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), - xhat.Y().value(), xhat.Rotation().Radians().value(), - groundTruthState.pose.X().value(), - groundTruthState.pose.Y().value(), - groundTruthState.pose.Rotation().Radians().value()); - } - - double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) - .value(); - - if (error > maxError) { - maxError = error; - } - errorSum += error; - - t += dt; - } - - if (debug) { - fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n"); - - units::second_t apply_time; - units::second_t measure_time; - frc::Pose2d vision_pose; - for (auto record : visionLog) { - std::tie(apply_time, measure_time, vision_pose) = record; - fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(), - measure_time.value(), vision_pose.X().value(), - vision_pose.Y().value(), - vision_pose.Rotation().Radians().value()); - } - } - - EXPECT_NEAR(endingPose.X().value(), - estimator.GetEstimatedPosition().X().value(), 0.08); - EXPECT_NEAR(endingPose.Y().value(), - estimator.GetEstimatedPosition().Y().value(), 0.08); - EXPECT_NEAR(endingPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), - 0.15); - - if (checkError) { - // NOLINTNEXTLINE(bugprone-integer-division) - EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051); - EXPECT_LT(maxError, 0.2); - } -} - -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::MecanumDriveWheelPositions wheelPositions; - - frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{}, - wheelPositions, frc::Pose2d{}, - {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; - - 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(2.0_mps, 2.0_mps_sq)); - - testFollowTrajectory( - kinematics, estimator, trajectory, - [&](frc::Trajectory::State& state) { - return frc::ChassisSpeeds{state.velocity, 0_mps, - state.velocity * state.curvature}; - }, - [&](frc::Trajectory::State& state) { return state.pose; }, - trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, - 0.1_s, 0.25_s, true, false); -} - -TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) { - 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{kinematics, frc::Rotation2d{}, - wheelPositions, frc::Pose2d{}, - {0.1, 0.1, 0.1}, {0.45, 0.45, 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(2.0_mps, 2.0_mps_sq)); - - for (units::degree_t offset_direction_degs = 0_deg; - offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) { - for (units::degree_t offset_heading_degs = 0_deg; - offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) { - auto pose_offset = frc::Rotation2d{offset_direction_degs}; - auto heading_offset = frc::Rotation2d{offset_heading_degs}; - - auto initial_pose = - trajectory.InitialPose() + - frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m, - pose_offset.Sin() * 1_m}, - heading_offset}; - - testFollowTrajectory( - kinematics, estimator, trajectory, - [&](frc::Trajectory::State& state) { - return frc::ChassisSpeeds{state.velocity, 0_mps, - state.velocity * state.curvature}; - }, - [&](frc::Trajectory::State& state) { return state.pose; }, - initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s, - 0.25_s, false, false); - } - } -} - -TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { - // This tests for multiple vision measurements appled at the same time. - // The expected behavior is that all measurements affect the estimated pose. - // The alternative result is that only one vision measurement affects the - // outcome. If that were the case, after 1000 measurements, the estimated - // pose would converge to that measurement. - 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{ - kinematics, frc::Rotation2d{}, - wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}, - {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}}; - - estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions); - - for (int i = 0; i < 1000; i++) { - estimator.AddVisionMeasurement( - frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s); - estimator.AddVisionMeasurement( - frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s); - estimator.AddVisionMeasurement( - frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s); - } - - { - auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m); - auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m); - auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg); - - EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); - } - - { - auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m); - auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m); - auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg); - - EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); - } - - { - auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m); - auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m); - auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg); - - EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); - } -} - -TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { - 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{ - kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{}, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; - - // Add enough measurements to fill up the buffer - for (auto time = 0.0_s; time < 4_s; time += 0.02_s) { - estimator.UpdateWithTime(time, frc::Rotation2d{}, - frc::MecanumDriveWheelPositions{}); - } - - auto odometryPose = estimator.GetEstimatedPosition(); - - // Apply a vision measurement from 3 seconds ago - estimator.AddVisionMeasurement( - frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}}, - 1_s, {0.1, 0.1, 0.1}); - - EXPECT_NEAR(odometryPose.X().value(), - estimator.GetEstimatedPosition().X().value(), 1e-6); - EXPECT_NEAR(odometryPose.Y().value(), - estimator.GetEstimatedPosition().Y().value(), 1e-6); - EXPECT_NEAR(odometryPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), - 1e-6); -} diff --git a/wpimath/src/test/native/cpp/estimator/PoseEstimationTestUtil.h b/wpimath/src/test/native/cpp/estimator/PoseEstimationTestUtil.h new file mode 100644 index 00000000000..093a63a0368 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/PoseEstimationTestUtil.h @@ -0,0 +1,65 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Twist2d.h" +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/Kinematics.h" +#include "frc/kinematics/WheelPositions.h" + +namespace frc { + +class SE2KinematicPrimitive { + public: + explicit constexpr SE2KinematicPrimitive(const Pose2d& pose) : m_pose{pose} {} + constexpr SE2KinematicPrimitive(const SE2KinematicPrimitive&) = default; + + SE2KinematicPrimitive& operator=(const SE2KinematicPrimitive&) = default; + + SE2KinematicPrimitive Interpolate(const SE2KinematicPrimitive& endValue, + double t) const { + auto twist = m_pose.Log(endValue.m_pose); + Twist2d scaled{twist.dx * t, twist.dy * t, twist.dtheta * t}; + + return SE2KinematicPrimitive{m_pose.Exp(scaled)}; + } + + constexpr const Pose2d& GetPose() const { return m_pose; } + + private: + Pose2d m_pose; +}; + +static_assert(frc::WheelPositions); + +class SE2Kinematics + : public Kinematics { + public: + explicit SE2Kinematics(units::second_t dt) : m_dt{dt} {} + + ChassisSpeeds ToChassisSpeeds( + const SE2KinematicPrimitive& wheelSpeeds) const override { + auto twist = Pose2d{}.Log(wheelSpeeds.GetPose()); + return ChassisSpeeds{twist.dx / m_dt, twist.dy / m_dt, twist.dtheta / m_dt}; + } + + SE2KinematicPrimitive ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds) const override { + return SE2KinematicPrimitive{ + Pose2d{}.Exp(Twist2d{chassisSpeeds.vx * m_dt, chassisSpeeds.vy * m_dt, + chassisSpeeds.omega * m_dt})}; + } + + Twist2d ToTwist2d(const SE2KinematicPrimitive& start, + const SE2KinematicPrimitive& end) const override { + return start.GetPose().Log(end.GetPose()); + } + + private: + units::second_t m_dt; +}; + +} // namespace frc diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/PoseEstimatorTest.cpp similarity index 60% rename from wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp rename to wpimath/src/test/native/cpp/estimator/PoseEstimatorTest.cpp index 3103068344d..8c6975c7b9d 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/PoseEstimatorTest.cpp @@ -7,34 +7,40 @@ #include #include +#include #include +#include "../SamplingUtil.h" +#include "PoseEstimationTestUtil.h" #include "frc/StateSpaceUtil.h" -#include "frc/estimator/DifferentialDrivePoseEstimator.h" +#include "frc/estimator/PoseEstimator.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" -#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/Odometry.h" #include "frc/trajectory/TrajectoryGenerator.h" #include "units/angle.h" #include "units/length.h" #include "units/time.h" -void testFollowTrajectory( - const frc::DifferentialDriveKinematics& kinematics, - frc::DifferentialDrivePoseEstimator& estimator, - const frc::Trajectory& trajectory, - std::function - chassisSpeedsGenerator, - std::function - visionMeasurementGenerator, - const frc::Pose2d& startingPose, const frc::Pose2d& endingPose, - const units::second_t dt, const units::second_t kVisionUpdateRate, - const units::second_t kVisionUpdateDelay, const bool checkError, - const bool debug) { - units::meter_t leftDistance = 0_m; - units::meter_t rightDistance = 0_m; - - estimator.ResetPosition(frc::Rotation2d{}, leftDistance, rightDistance, +namespace frc { + +using SE2Odometry = Odometry; +using SE2PoseEstimator = + PoseEstimator; + +} // namespace frc + +void testFollowTrajectory(const frc::SE2Kinematics& kinematics, + frc::SE2PoseEstimator& estimator, + const frc::Trajectory& trajectory, + const frc::Pose2d& startingPose, + const frc::Pose2d& endingPose, + const units::second_t dt, + const units::second_t kVisionUpdateRate, + const units::second_t kVisionUpdateDelay, + const bool checkError, const bool debug) { + frc::SE2KinematicPrimitive wheelPositions{frc::Pose2d{}}; + estimator.ResetPosition(trajectory.InitialPose().Rotation(), wheelPositions, startingPose); std::default_random_engine generator; @@ -50,9 +56,7 @@ void testFollowTrajectory( double errorSum = 0; if (debug) { - fmt::print( - "time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, " - "right\n"); + fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n"); } while (t < trajectory.TotalTime()) { @@ -62,11 +66,9 @@ void testFollowTrajectory( // seconds since the last vision measurement if (visionPoses.empty() || visionPoses.back().first + kVisionUpdateRate < t) { - auto visionPose = - visionMeasurementGenerator(groundTruthState) + - frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m, - distribution(generator) * 0.1_m}, - frc::Rotation2d{distribution(generator) * 0.05_rad}}; + auto adjustment = + frc::SampleTwist2d(generator, distribution, {0.1, 0.1, 0.05}); + auto visionPose = groundTruthState.pose.Exp(adjustment); visionPoses.push_back({t, visionPose}); } @@ -80,27 +82,25 @@ void testFollowTrajectory( visionLog.push_back({t, visionEntry.first, visionEntry.second}); } - auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState); + auto gyroAngle = groundTruthState.pose.Rotation() + + frc::SampleRotation2d(generator, distribution, 0.05); - auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); + frc::Twist2d trueDelta{ + groundTruthState.velocity * dt, 0_m, + groundTruthState.velocity * groundTruthState.curvature * dt}; - leftDistance += wheelSpeeds.left * dt; - rightDistance += wheelSpeeds.right * dt; + wheelPositions = frc::SE2KinematicPrimitive{ + wheelPositions.GetPose().Exp(trueDelta).Exp(frc::SampleTwist2d( + generator, distribution, {0.001, 0.001, 0.005}))}; - auto xhat = estimator.UpdateWithTime( - t, - groundTruthState.pose.Rotation() + - frc::Rotation2d{distribution(generator) * 0.05_rad} - - trajectory.InitialPose().Rotation(), - leftDistance, rightDistance); + auto xhat = estimator.UpdateWithTime(t, gyroAngle, wheelPositions); if (debug) { - fmt::print( - "{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), - xhat.Y().value(), xhat.Rotation().Radians().value(), - groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(), - groundTruthState.pose.Rotation().Radians().value(), - leftDistance.value(), rightDistance.value()); + fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), + xhat.Y().value(), xhat.Rotation().Radians().value(), + groundTruthState.pose.X().value(), + groundTruthState.pose.Y().value(), + groundTruthState.pose.Rotation().Radians().value()); } double error = groundTruthState.pose.Translation() @@ -131,12 +131,12 @@ void testFollowTrajectory( } EXPECT_NEAR(endingPose.X().value(), - estimator.GetEstimatedPosition().X().value(), 0.08); + estimator.GetEstimatedPosition().X().value(), 0.06); EXPECT_NEAR(endingPose.Y().value(), - estimator.GetEstimatedPosition().Y().value(), 0.08); + estimator.GetEstimatedPosition().Y().value(), 0.06); EXPECT_NEAR(endingPose.Rotation().Radians().value(), estimator.GetEstimatedPosition().Rotation().Radians().value(), - 0.15); + 0.10); if (checkError) { // NOLINTNEXTLINE(bugprone-integer-division) @@ -145,12 +145,13 @@ void testFollowTrajectory( } } -TEST(DifferentialDrivePoseEstimatorTest, Accuracy) { - frc::DifferentialDriveKinematics kinematics{1.0_m}; - - frc::DifferentialDrivePoseEstimator estimator{ - kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, - {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}}; +TEST(PoseEstimatorTest, Accuracy) { + frc::SE2Kinematics kinematics{0.02_s}; + frc::SE2Odometry odometry{kinematics, frc::Rotation2d{}, + frc::SE2KinematicPrimitive{frc::Pose2d{}}, + frc::Pose2d{}}; + frc::SE2PoseEstimator estimator{ + odometry, {0.02, 0.02, 0.01}, {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}, @@ -159,23 +160,19 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) { frc::Pose2d{0_m, 0_m, 45_deg}}, frc::TrajectoryConfig(2_mps, 2_mps_sq)); - testFollowTrajectory( - kinematics, estimator, trajectory, - [&](frc::Trajectory::State& state) { - return frc::ChassisSpeeds{state.velocity, 0_mps, - state.velocity * state.curvature}; - }, - [&](frc::Trajectory::State& state) { return state.pose; }, - trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, - 0.1_s, 0.25_s, true, false); + testFollowTrajectory(kinematics, estimator, trajectory, + trajectory.InitialPose(), + trajectory.Sample(trajectory.TotalTime()).pose, 0.02_s, + 0.1_s, 0.25_s, true, false); } -TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) { - frc::DifferentialDriveKinematics kinematics{1.0_m}; - - frc::DifferentialDrivePoseEstimator estimator{ - kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, - {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}}; +TEST(PoseEstimatorTest, BadInitialPose) { + frc::SE2Kinematics kinematics{0.02_s}; + frc::SE2Odometry odometry{kinematics, frc::Rotation2d{}, + frc::SE2KinematicPrimitive{frc::Pose2d{}}, + frc::Pose2d{}}; + frc::SE2PoseEstimator estimator{ + odometry, {0.02, 0.02, 0.01}, {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}, @@ -197,37 +194,28 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) { pose_offset.Sin() * 1_m}, heading_offset}; - testFollowTrajectory( - kinematics, estimator, trajectory, - [&](frc::Trajectory::State& state) { - return frc::ChassisSpeeds{state.velocity, 0_mps, - state.velocity * state.curvature}; - }, - [&](frc::Trajectory::State& state) { return state.pose; }, - initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s, - 0.25_s, false, false); + testFollowTrajectory(kinematics, estimator, trajectory, initial_pose, + trajectory.Sample(trajectory.TotalTime()).pose, + 0.02_s, 0.1_s, 0.25_s, false, false); } } } -TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { +TEST(PoseEstimatorTest, SimultaneousVisionMeasurements) { // This tests for multiple vision measurements appled at the same time. // The expected behavior is that all measurements affect the estimated pose. // The alternative result is that only one vision measurement affects the // outcome. If that were the case, after 1000 measurements, the estimated // pose would converge to that measurement. - frc::DifferentialDriveKinematics kinematics{1.0_m}; + frc::SE2Kinematics kinematics{0.02_s}; + frc::SE2Odometry odometry{kinematics, frc::Rotation2d{}, + frc::SE2KinematicPrimitive{frc::Pose2d{}}, + frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}}; + frc::SE2PoseEstimator estimator{ + odometry, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}}; - frc::DifferentialDrivePoseEstimator estimator{ - kinematics, - frc::Rotation2d{}, - 0_m, - 0_m, - frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}, - {0.02, 0.02, 0.01}, - {0.1, 0.1, 0.1}}; - - estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m); + estimator.UpdateWithTime(0_s, frc::Rotation2d{}, + frc::SE2KinematicPrimitive{frc::Pose2d{}}); for (int i = 0; i < 1000; i++) { estimator.AddVisionMeasurement( @@ -266,16 +254,18 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { } } -TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { - frc::DifferentialDriveKinematics kinematics{1_m}; - - frc::DifferentialDrivePoseEstimator estimator{ - kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, - {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; +TEST(PoseEstimatorTest, TestDiscardStaleVisionMeasurements) { + frc::SE2Kinematics kinematics{0.02_s}; + frc::SE2Odometry odometry{kinematics, frc::Rotation2d{}, + frc::SE2KinematicPrimitive{frc::Pose2d{}}, + frc::Pose2d{}}; + frc::SE2PoseEstimator estimator{ + odometry, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; // Add enough measurements to fill up the buffer for (auto time = 0.0_s; time < 4_s; time += 0.02_s) { - estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m); + estimator.UpdateWithTime(time, frc::Rotation2d{}, + frc::SE2KinematicPrimitive{frc::Pose2d{}}); } auto odometryPose = estimator.GetEstimatedPosition(); diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp deleted file mode 100644 index f1024efbc92..00000000000 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ /dev/null @@ -1,313 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include -#include -#include -#include - -#include -#include -#include - -#include "frc/estimator/SwerveDrivePoseEstimator.h" -#include "frc/geometry/Pose2d.h" -#include "frc/kinematics/SwerveDriveKinematics.h" -#include "frc/trajectory/TrajectoryGenerator.h" - -void testFollowTrajectory( - const frc::SwerveDriveKinematics<4>& kinematics, - frc::SwerveDrivePoseEstimator<4>& estimator, - const frc::Trajectory& trajectory, - std::function - chassisSpeedsGenerator, - std::function - visionMeasurementGenerator, - const frc::Pose2d& startingPose, const frc::Pose2d& endingPose, - const units::second_t dt, const units::second_t kVisionUpdateRate, - const units::second_t kVisionUpdateDelay, const bool checkError, - const bool debug) { - wpi::array positions{wpi::empty_array}; - - estimator.ResetPosition(frc::Rotation2d{}, positions, startingPose); - - std::default_random_engine generator; - std::normal_distribution distribution(0.0, 1.0); - - units::second_t t = 0_s; - - std::vector> visionPoses; - std::vector> - visionLog; - - double maxError = -std::numeric_limits::max(); - double errorSum = 0; - - if (debug) { - fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n"); - } - - while (t < trajectory.TotalTime()) { - frc::Trajectory::State groundTruthState = trajectory.Sample(t); - - // We are due for a new vision measurement if it's been `visionUpdateRate` - // seconds since the last vision measurement - if (visionPoses.empty() || - visionPoses.back().first + kVisionUpdateRate < t) { - auto visionPose = - visionMeasurementGenerator(groundTruthState) + - frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m, - distribution(generator) * 0.1_m}, - frc::Rotation2d{distribution(generator) * 0.05_rad}}; - visionPoses.push_back({t, visionPose}); - } - - // We should apply the oldest vision measurement if it has been - // `visionUpdateDelay` seconds since it was measured - if (!visionPoses.empty() && - visionPoses.front().first + kVisionUpdateDelay < t) { - auto visionEntry = visionPoses.front(); - estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first); - visionPoses.erase(visionPoses.begin()); - visionLog.push_back({t, visionEntry.first, visionEntry.second}); - } - - auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState); - - auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds); - - for (size_t i = 0; i < 4; i++) { - positions[i].distance += moduleStates[i].speed * dt; - positions[i].angle = moduleStates[i].angle; - } - - auto xhat = estimator.UpdateWithTime( - t, - groundTruthState.pose.Rotation() + - frc::Rotation2d{distribution(generator) * 0.05_rad} - - trajectory.InitialPose().Rotation(), - positions); - - if (debug) { - fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), - xhat.Y().value(), xhat.Rotation().Radians().value(), - groundTruthState.pose.X().value(), - groundTruthState.pose.Y().value(), - groundTruthState.pose.Rotation().Radians().value()); - } - - double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) - .value(); - - if (error > maxError) { - maxError = error; - } - errorSum += error; - - t += dt; - } - - if (debug) { - fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n"); - - units::second_t apply_time; - units::second_t measure_time; - frc::Pose2d vision_pose; - for (auto record : visionLog) { - std::tie(apply_time, measure_time, vision_pose) = record; - fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(), - measure_time.value(), vision_pose.X().value(), - vision_pose.Y().value(), - vision_pose.Rotation().Radians().value()); - } - } - - EXPECT_NEAR(endingPose.X().value(), - estimator.GetEstimatedPosition().X().value(), 0.08); - EXPECT_NEAR(endingPose.Y().value(), - estimator.GetEstimatedPosition().Y().value(), 0.08); - EXPECT_NEAR(endingPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), - 0.15); - - if (checkError) { - // NOLINTNEXTLINE(bugprone-integer-division) - EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058); - EXPECT_LT(maxError, 0.2); - } -} - -TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) { - frc::SwerveDriveKinematics<4> kinematics{ - frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, - frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; - - frc::SwerveModulePosition fl; - frc::SwerveModulePosition fr; - frc::SwerveModulePosition bl; - frc::SwerveModulePosition br; - - frc::SwerveDrivePoseEstimator<4> estimator{ - kinematics, frc::Rotation2d{}, {fl, fr, bl, br}, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; - - 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(2_mps, 2.0_mps_sq)); - - testFollowTrajectory( - kinematics, estimator, trajectory, - [&](frc::Trajectory::State& state) { - return frc::ChassisSpeeds{state.velocity, 0_mps, - state.velocity * state.curvature}; - }, - [&](frc::Trajectory::State& state) { return state.pose; }, - {0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}}, - 0.02_s, 0.1_s, 0.25_s, true, false); -} - -TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) { - frc::SwerveDriveKinematics<4> kinematics{ - frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, - frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; - - frc::SwerveModulePosition fl; - frc::SwerveModulePosition fr; - frc::SwerveModulePosition bl; - frc::SwerveModulePosition br; - - frc::SwerveDrivePoseEstimator<4> estimator{ - kinematics, frc::Rotation2d{}, {fl, fr, bl, br}, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}}; - - 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(2_mps, 2.0_mps_sq)); - - for (units::degree_t offset_direction_degs = 0_deg; - offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) { - for (units::degree_t offset_heading_degs = 0_deg; - offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) { - auto pose_offset = frc::Rotation2d{offset_direction_degs}; - auto heading_offset = frc::Rotation2d{offset_heading_degs}; - - auto initial_pose = - trajectory.InitialPose() + - frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m, - pose_offset.Sin() * 1_m}, - heading_offset}; - - testFollowTrajectory( - kinematics, estimator, trajectory, - [&](frc::Trajectory::State& state) { - return frc::ChassisSpeeds{state.velocity, 0_mps, - state.velocity * state.curvature}; - }, - [&](frc::Trajectory::State& state) { return state.pose; }, - initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s, - 0.25_s, false, false); - } - } -} - -TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { - // This tests for multiple vision measurements appled at the same time. - // The expected behavior is that all measurements affect the estimated pose. - // The alternative result is that only one vision measurement affects the - // outcome. If that were the case, after 1000 measurements, the estimated - // pose would converge to that measurement. - frc::SwerveDriveKinematics<4> kinematics{ - frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, - frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; - - frc::SwerveModulePosition fl; - frc::SwerveModulePosition fr; - frc::SwerveModulePosition bl; - frc::SwerveModulePosition br; - - frc::SwerveDrivePoseEstimator<4> estimator{ - kinematics, frc::Rotation2d{}, - {fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}, - {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; - - estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br}); - - for (int i = 0; i < 1000; i++) { - estimator.AddVisionMeasurement( - frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s); - estimator.AddVisionMeasurement( - frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s); - estimator.AddVisionMeasurement( - frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s); - } - - { - auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m); - auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m); - auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg); - - EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); - } - - { - auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m); - auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m); - auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg); - - EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); - } - - { - auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m); - auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m); - auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg); - - EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); - } -} - -TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { - frc::SwerveDriveKinematics<4> kinematics{ - frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, - frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; - - frc::SwerveModulePosition fl; - frc::SwerveModulePosition fr; - frc::SwerveModulePosition bl; - frc::SwerveModulePosition br; - - frc::SwerveDrivePoseEstimator<4> estimator{ - kinematics, frc::Rotation2d{}, {fl, fr, bl, br}, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; - - // Add enough measurements to fill up the buffer - for (auto time = 0.0_s; time < 4_s; time += 0.02_s) { - estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br}); - } - - auto odometryPose = estimator.GetEstimatedPosition(); - - // Apply a vision measurement from 3 seconds ago - estimator.AddVisionMeasurement( - frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}}, - 1_s, {0.1, 0.1, 0.1}); - - EXPECT_NEAR(odometryPose.X().value(), - estimator.GetEstimatedPosition().X().value(), 1e-6); - EXPECT_NEAR(odometryPose.Y().value(), - estimator.GetEstimatedPosition().Y().value(), 1e-6); - EXPECT_NEAR(odometryPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), - 1e-6); -}