From 7006866d87b1549964a2ea980e2c5eaea2568dea Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Thu, 15 Feb 2024 20:04:48 -0500 Subject: [PATCH] Use transforms when applying vision pose (#13) Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- .../littletonrobotics/frc2024/RobotState.java | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 3e3fb245..98119d50 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -153,15 +153,21 @@ public void addVisionObservation(VisionObservation observation) { } } // difference between estimate and vision pose - Twist2d twist = estimateAtTime.log(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)); + Transform2d transform = new Transform2d(estimateAtTime, observation.visionPose()); + // scale transform by visionK + 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) {