From f15516442f860591187a0b77ad79a8065b89de94 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Fri, 21 Oct 2022 10:58:27 -0500 Subject: [PATCH 1/3] Update simposeest to use 3d Fixes #516 --- .../examples/simposeest/robot/Constants.java | 22 +++++++++++++------ .../robot/DrivetrainPoseEstimator.java | 17 ++++++-------- .../simposeest/sim/DrivetrainSim.java | 4 +++- 3 files changed, 25 insertions(+), 18 deletions(-) diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java index 6afee4fa59..9f7e384176 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java @@ -25,9 +25,13 @@ package org.photonlib.examples.simposeest.robot; 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.Transform2d; +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.util.Units; import org.photonvision.SimVisionTarget; @@ -71,10 +75,10 @@ public class Constants { // Physical location of the camera on the robot, relative to the center of the // robot. - public static final Transform2d kCameraToRobot = - new Transform2d( - new Translation2d(-0.25, 0), // in meters - new Rotation2d()); + public static final Transform3d kCameraToRobot = + new Transform3d( + new Translation3d(-0.25, 0, -.25), // in meters + new Rotation3d()); // See // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf @@ -94,9 +98,13 @@ public class Constants { public static final double kFarTgtXPos = Units.feetToMeters(54); public static final double kFarTgtYPos = Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0); - public static final Pose2d kFarTargetPose = - new Pose2d(new Translation2d(kFarTgtXPos, kFarTgtYPos), new Rotation2d(0.0)); + public static final double kFarTgtZPos = + (Units.inchesToMeters(98.19) - targetHeight) / 2 + targetHeight; + + public static final Pose3d kFarTargetPose = new Pose3d( + new Translation3d(kFarTgtXPos, kFarTgtYPos, kFarTgtZPos), + new Rotation3d(0.0, 0.0, 0.0)); public static final SimVisionTarget kFarTarget = - new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, targetWidth, targetHeight); + new SimVisionTarget(kFarTargetPose.toPose2d(), targetHeightAboveGround, targetWidth, targetHeight); } diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java index c9e6480c49..1f9ac4df8d 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java @@ -24,11 +24,13 @@ package org.photonlib.examples.simposeest.robot; +import org.photonvision.PhotonCamera; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.numbers.N1; @@ -37,7 +39,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Timer; -import org.photonvision.PhotonCamera; /** * Performs estimation of the drivetrain's current position on the field, using a vision system, @@ -86,15 +87,11 @@ public void update( var res = cam.getLatestResult(); if (res.hasTargets()) { - double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0; - Transform3d camToTargetTrans = res.getBestTarget().getCameraToTarget(); - var transform = - new Transform2d( - camToTargetTrans.getTranslation().toTranslation2d(), - camToTargetTrans.getRotation().toRotation2d()); - Pose2d camPose = Constants.kFarTargetPose.transformBy(transform.inverse()); + var imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0; + var camToTargetTrans = res.getBestTarget().getCameraToTarget(); + var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse()); m_poseEstimator.addVisionMeasurement( - camPose.transformBy(Constants.kCameraToRobot), imageCaptureTime); + camPose.transformBy(Constants.kCameraToRobot).toPose2d(), imageCaptureTime); } } diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java index 91c2c7054f..01e84e45ad 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java @@ -90,7 +90,9 @@ public class DrivetrainSim { Constants.kCamName, camDiagFOV, camPitch, - Constants.kCameraToRobot, + new Transform2d( + Constants.kCameraToRobot.getTranslation().toTranslation2d(), + Constants.kCameraToRobot.getRotation().toRotation2d()), camHeightOffGround, maxLEDRange, camResolutionWidth, From 1eb309f3963768236c0a979362a828eb25ce5230 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Fri, 21 Oct 2022 11:21:18 -0500 Subject: [PATCH 2/3] Resolve lint issues --- .../examples/simposeest/robot/Constants.java | 13 +++++-------- .../simposeest/robot/DrivetrainPoseEstimator.java | 5 +---- .../examples/simposeest/sim/DrivetrainSim.java | 4 ++-- 3 files changed, 8 insertions(+), 14 deletions(-) diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java index 9f7e384176..679b27ed8e 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java @@ -24,13 +24,9 @@ package org.photonlib.examples.simposeest.robot; -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.Transform2d; 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.util.Units; import org.photonvision.SimVisionTarget; @@ -101,10 +97,11 @@ public class Constants { public static final double kFarTgtZPos = (Units.inchesToMeters(98.19) - targetHeight) / 2 + targetHeight; - public static final Pose3d kFarTargetPose = new Pose3d( - new Translation3d(kFarTgtXPos, kFarTgtYPos, kFarTgtZPos), - new Rotation3d(0.0, 0.0, 0.0)); + public static final Pose3d kFarTargetPose = + new Pose3d( + new Translation3d(kFarTgtXPos, kFarTgtYPos, kFarTgtZPos), new Rotation3d(0.0, 0.0, 0.0)); public static final SimVisionTarget kFarTarget = - new SimVisionTarget(kFarTargetPose.toPose2d(), targetHeightAboveGround, targetWidth, targetHeight); + new SimVisionTarget( + kFarTargetPose.toPose2d(), targetHeightAboveGround, targetWidth, targetHeight); } diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java index 1f9ac4df8d..1e126a0c2d 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java @@ -24,14 +24,10 @@ package org.photonlib.examples.simposeest.robot; -import org.photonvision.PhotonCamera; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -39,6 +35,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Timer; +import org.photonvision.PhotonCamera; /** * Performs estimation of the drivetrain's current position on the field, using a vision system, diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java index 01e84e45ad..941c6a7eb2 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java @@ -91,8 +91,8 @@ public class DrivetrainSim { camDiagFOV, camPitch, new Transform2d( - Constants.kCameraToRobot.getTranslation().toTranslation2d(), - Constants.kCameraToRobot.getRotation().toRotation2d()), + Constants.kCameraToRobot.getTranslation().toTranslation2d(), + Constants.kCameraToRobot.getRotation().toRotation2d()), camHeightOffGround, maxLEDRange, camResolutionWidth, From b6184f59ecffed260f0c195ba7602f71cf9a2e2a Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Mon, 24 Oct 2022 11:10:06 -0500 Subject: [PATCH 3/3] Target z rotation is 180 --- .../org/photonlib/examples/simposeest/robot/Constants.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java index 679b27ed8e..a13fa154bd 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java @@ -99,7 +99,8 @@ public class Constants { public static final Pose3d kFarTargetPose = new Pose3d( - new Translation3d(kFarTgtXPos, kFarTgtYPos, kFarTgtZPos), new Rotation3d(0.0, 0.0, 0.0)); + new Translation3d(kFarTgtXPos, kFarTgtYPos, kFarTgtZPos), + new Rotation3d(0.0, 0.0, Units.degreesToRadians(180))); public static final SimVisionTarget kFarTarget = new SimVisionTarget(