From 080d70e0574c89200858e1e035a23f3dcf985ca0 Mon Sep 17 00:00:00 2001 From: suryatho Date: Tue, 13 Feb 2024 21:05:09 -0500 Subject: [PATCH 1/2] Use transforms when applying vision pose --- .../littletonrobotics/frc2024/RobotState.java | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index f9f22714..80f9d2a1 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -143,15 +143,21 @@ public void addVisionObservation(VisionObservation observation) { } } // difference between estimate and vision pose - Twist2d twist = estimateAtTime.log(observation.visionPose()); + Transform2d transform = new Transform2d(estimateAtTime, observation.visionPose()); // scale twist by visionK - var kTimesTwist = visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta)); - Twist2d scaledTwist = - new Twist2d(kTimesTwist.get(0, 0), kTimesTwist.get(1, 0), kTimesTwist.get(2, 0)); + var kTimesTransform = + visionK.times( + VecBuilder.fill( + transform.getX(), transform.getY(), transform.getRotation().getRadians())); + Transform2d scaledTransform = + new Transform2d( + kTimesTransform.get(0, 0), + kTimesTransform.get(1, 0), + Rotation2d.fromRadians(kTimesTransform.get(2, 0))); // Recalculate current estimate by applying scaled twist to old estimate // then replaying odometry data - estimatedPose = estimateAtTime.exp(scaledTwist).plus(sampleToOdometryTransform); + estimatedPose = estimateAtTime.plus(scaledTransform).plus(sampleToOdometryTransform); } public void addVelocityData(Twist2d robotVelocity) { From 82f0db4dec791c8a9073b280e7741dd485febe57 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Thu, 15 Feb 2024 20:00:57 -0500 Subject: [PATCH 2/2] Fix comment --- src/main/java/org/littletonrobotics/frc2024/RobotState.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 80f9d2a1..430d403e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -144,7 +144,7 @@ public void addVisionObservation(VisionObservation observation) { } // difference between estimate and vision pose Transform2d transform = new Transform2d(estimateAtTime, observation.visionPose()); - // scale twist by visionK + // scale transform by visionK var kTimesTransform = visionK.times( VecBuilder.fill(