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 1f00cb7b18..4e9bf4b3e8 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 @@ -11,8 +11,8 @@ import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry; @@ -273,19 +273,27 @@ public class PoseEstimator { return; } - // Step 4: Measure the twist between the old pose estimate and the vision pose. - var twist = visionSample.get().log(visionRobotPoseMeters); + // Step 4: Measure the transform between the old pose estimate and the vision pose. + var transform = visionRobotPoseMeters.minus(visionSample.get()); - // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // Step 5: We should not trust the transform entirely, so instead we scale this transform 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)); + var k_times_transform = + m_visionK.times( + VecBuilder.fill( + transform.getX(), transform.getY(), transform.getRotation().getRadians())); - // 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 6: Convert back to Transform2d. + var scaledTransform = + new Transform2d( + k_times_transform.get(0, 0), + k_times_transform.get(1, 0), + Rotation2d.fromRadians(k_times_transform.get(2, 0))); // Step 7: Calculate and record the vision update. - var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); + var visionUpdate = + new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get()); m_visionUpdates.put(timestampSeconds, visionUpdate); // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java index 9ce96f0243..463cb9481c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -13,9 +13,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry3d; @@ -286,27 +286,36 @@ public class PoseEstimator3d { return; } - // Step 4: Measure the twist between the old pose estimate and the vision pose. - var twist = visionSample.get().log(visionRobotPoseMeters); + // Step 4: Measure the transform between the old pose estimate and the vision pose. + var transform = visionRobotPoseMeters.minus(visionSample.get()); - // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // Step 5: We should not trust the transform entirely, so instead we scale this transform by a + // Kalman // gain matrix representing how much we trust vision measurements compared to our current pose. - var k_times_twist = + var k_times_transform = m_visionK.times( - VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz)); + VecBuilder.fill( + transform.getX(), + transform.getY(), + transform.getZ(), + transform.getRotation().getX(), + transform.getRotation().getY(), + transform.getRotation().getZ())); - // Step 6: Convert back to Twist3d. - var scaledTwist = - new Twist3d( - k_times_twist.get(0, 0), - k_times_twist.get(1, 0), - k_times_twist.get(2, 0), - k_times_twist.get(3, 0), - k_times_twist.get(4, 0), - k_times_twist.get(5, 0)); + // Step 6: Convert back to Transform3d. + var scaledTransform = + new Transform3d( + k_times_transform.get(0, 0), + k_times_transform.get(1, 0), + k_times_transform.get(2, 0), + new Rotation3d( + k_times_transform.get(3, 0), + k_times_transform.get(4, 0), + k_times_transform.get(5, 0))); // Step 7: Calculate and record the vision update. - var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); + var visionUpdate = + new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get()); m_visionUpdates.put(timestampSeconds, visionUpdate); // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 9c3bc4d6de..1aaf4a39d8 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -14,6 +14,7 @@ #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" #include "frc/geometry/Translation2d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/Kinematics.h" @@ -261,24 +262,26 @@ class WPILIB_DLLEXPORT PoseEstimator { return; } - // Step 4: Measure the twist between the old pose estimate and the vision - // pose. - auto twist = visionSample.value().Log(visionRobotPose); + // Step 4: Measure the transform between the old pose estimate and the + // vision transform. + auto transform = visionRobotPose - visionSample.value(); - // 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 5: We should not trust the transform entirely, so instead we scale + // this transform by a Kalman gain matrix representing how much we trust + // vision measurements compared to our current pose. + Eigen::Vector3d k_times_transform = + m_visionK * Eigen::Vector3d{transform.X().value(), + transform.Y().value(), + transform.Rotation().Radians().value()}; - // 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 6: Convert back to Transform2d. + Transform2d scaledTransform{ + units::meter_t{k_times_transform(0)}, + units::meter_t{k_times_transform(1)}, + Rotation2d{units::radian_t{k_times_transform(2)}}}; // Step 7: Calculate and record the vision update. - VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample}; + VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample}; m_visionUpdates[timestamp] = visionUpdate; // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h index bc8bdb08a6..ecf9ccbf88 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -14,9 +14,10 @@ #include #include "frc/EigenCore.h" -#include "frc/geometry/Pose2d.h" -#include "frc/geometry/Rotation2d.h" -#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" +#include "frc/geometry/Transform3d.h" +#include "frc/geometry/Translation3d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/Kinematics.h" #include "frc/kinematics/Odometry3d.h" @@ -270,26 +271,32 @@ class WPILIB_DLLEXPORT PoseEstimator3d { return; } - // Step 4: Measure the twist between the old pose estimate and the vision - // pose. - auto twist = visionSample.value().Log(visionRobotPose); + // Step 4: Measure the transform between the old pose estimate and the + // vision pose. + auto transform = visionRobotPose - visionSample.value(); - // Step 5: We should not trust the twist entirely, so instead we scale this - // twist by a Kalman gain matrix representing how much we trust vision - // measurements compared to our current pose. - frc::Vectord<6> k_times_twist = - m_visionK * frc::Vectord<6>{twist.dx.value(), twist.dy.value(), - twist.dz.value(), twist.rx.value(), - twist.ry.value(), twist.rz.value()}; + // Step 5: We should not trust the transform entirely, so instead we scale + // this transform by a Kalman gain matrix representing how much we trust + // vision measurements compared to our current pose. + frc::Vectord<6> k_times_transform = + m_visionK * frc::Vectord<6>{transform.X().value(), + transform.Y().value(), + transform.Z().value(), + transform.Rotation().X().value(), + transform.Rotation().Y().value(), + transform.Rotation().Z().value()}; - // Step 6: Convert back to Twist3d. - Twist3d scaledTwist{ - units::meter_t{k_times_twist(0)}, units::meter_t{k_times_twist(1)}, - units::meter_t{k_times_twist(2)}, units::radian_t{k_times_twist(3)}, - units::radian_t{k_times_twist(4)}, units::radian_t{k_times_twist(5)}}; + // Step 6: Convert back to Transform3d. + Transform3d scaledTransform{ + units::meter_t{k_times_transform(0)}, + units::meter_t{k_times_transform(1)}, + units::meter_t{k_times_transform(2)}, + Rotation3d{units::radian_t{k_times_transform(3)}, + units::radian_t{k_times_transform(4)}, + units::radian_t{k_times_transform(5)}}}; // Step 7: Calculate and record the vision update. - VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample}; + VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample}; m_visionUpdates[timestamp] = visionUpdate; // Step 8: Remove later vision measurements. (Matches previous behavior)