From 7cdfe7bd377e3b1790465df7a2c45ad079d98510 Mon Sep 17 00:00:00 2001 From: David Vo Date: Sun, 10 Mar 2024 22:02:00 +1100 Subject: [PATCH 1/2] vision: Use wpimath.objectToRobotPose in one-tag mode --- components/vision.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/components/vision.py b/components/vision.py index 5806169b..25c34e9e 100644 --- a/components/vision.py +++ b/components/vision.py @@ -7,6 +7,7 @@ from magicbot import tunable from photonlibpy.photonCamera import PhotonCamera from photonlibpy.photonTrackedTarget import PhotonTrackedTarget +from wpimath import objectToRobotPose from wpimath.geometry import Pose2d, Rotation3d, Transform3d, Translation3d, Pose3d from components.chassis import ChassisComponent @@ -43,7 +44,8 @@ def __init__( chassis: ChassisComponent, ) -> None: self.camera = PhotonCamera(name) - self.camera_to_robot = Transform3d(pos, rot).inverse() + self.robot_to_camera = Transform3d(pos, rot) + self.camera_to_robot = self.robot_to_camera.inverse() self.last_timestamp = -1 self.last_recieved_timestep = -1.0 @@ -111,7 +113,7 @@ def execute(self) -> None: if target.getPoseAmbiguity() > 0.25: continue - poses = estimate_poses_from_apriltag(self.camera_to_robot, target) + poses = estimate_poses_from_apriltag(self.robot_to_camera, target) if poses is None: # tag doesn't exist continue @@ -151,22 +153,20 @@ def sees_target(self): def estimate_poses_from_apriltag( - cam_to_robot: Transform3d, target: PhotonTrackedTarget + robot_to_camera: Transform3d, target: PhotonTrackedTarget ) -> Optional[tuple[Pose2d, Pose2d, float]]: tag_id = target.getFiducialId() tag_pose = apriltag_layout.getTagPose(tag_id) if tag_pose is None: return None - best_pose = tag_pose.transformBy( - target.getBestCameraToTarget().inverse() - ).transformBy(cam_to_robot) - alternate_pose = ( - tag_pose.transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(cam_to_robot) - .toPose2d() + best_pose = objectToRobotPose( + tag_pose, target.getBestCameraToTarget(), robot_to_camera ) - return best_pose.toPose2d(), alternate_pose, best_pose.z + alternate_pose = objectToRobotPose( + tag_pose, target.getAlternateCameraToTarget(), robot_to_camera + ) + return best_pose.toPose2d(), alternate_pose.toPose2d(), best_pose.z def get_target_skew(target: PhotonTrackedTarget) -> float: From b0f102a029e2555d1f39f6bed2c716c37713f4b9 Mon Sep 17 00:00:00 2001 From: David Vo Date: Sun, 10 Mar 2024 22:03:22 +1100 Subject: [PATCH 2/2] vision: Remove redundant .radians() in Pose2d conversion --- components/vision.py | 20 ++++---------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/components/vision.py b/components/vision.py index 25c34e9e..98a71bc1 100644 --- a/components/vision.py +++ b/components/vision.py @@ -94,18 +94,10 @@ def execute(self) -> None: if self.should_log: self.multi_best_log.setPose( - Pose2d( - p.best.x, - p.best.y, - p.best.rotation().toRotation2d().radians(), - ) + Pose2d(p.best.x, p.best.y, p.best.rotation().toRotation2d()) ) self.multi_alt_log.setPose( - Pose2d( - p.alt.x, - p.alt.y, - p.alt.rotation().toRotation2d().radians(), - ) + Pose2d(p.alt.x, p.alt.y, p.alt.rotation().toRotation2d()) ) else: for target in results.getTargets(): @@ -133,18 +125,14 @@ def execute(self) -> None: Pose2d( target.bestCameraToTarget.x, target.bestCameraToTarget.y, - target.bestCameraToTarget.rotation() - .toRotation2d() - .radians(), + target.bestCameraToTarget.rotation().toRotation2d(), ) ) self.single_alt_log.setPose( Pose2d( target.altCameraToTarget.x, target.altCameraToTarget.y, - target.altCameraToTarget.rotation() - .toRotation2d() - .radians(), + target.altCameraToTarget.rotation().toRotation2d(), ) )