8000 Add distance, yaw, and robot pose methods to photonlib by mdurrani808 · Pull Request #642 · PhotonVision/photonvision · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Add distance, yaw, and robot pose methods to photonlib #642

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 6 commits into from
Dec 25, 2022
Merged

Add distance, yaw, and robot pose methods to photonlib #642

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
2b5b4ff
started testing
mdurrani808 Dec 20, 2022
03c4baf
photonlib updates
mdurrani808 Dec 21, 2022
d138f3d
spotless
mdurrani808 Dec 21, 2022
97c8e76
address issues
mdurrani808 Dec 22, 2022
9717dfb
Merge branch 'master' into newLib
mdurrani808 Dec 22, 2022
e78a6bf
Merge branch 'master' into newLib
mcm001 Dec 25, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 40 additions & 4 deletions photon-lib/src/main/java/org/photonvision/PhotonUtils.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,7 @@

package org.photonvision;

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.*;

public final class PhotonUtils {
private PhotonUtils() {
Expand Down Expand Up @@ -170,4 +167,43 @@ public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fi
var targetToCamera = cameraToTarget.inverse();
return fieldToTarget.transformBy(targetToCamera);
}

/**
* Estimates the pose of the robot in the field coordinate system, given the pose of the fiducial
* tag, the robot relative to the camera, and the target relative to the camera.
*
* @param fieldRelativeTagPose Pose3D the field relative pose of the target
* @param cameraToRobot Transform3D of the robot relative to the camera. Origin of the robot is
* defined as the center.
* @param cameraToTarget Transform3D of the target relative to the camera, returned by
* PhotonVision
* @return Transform3d Robot position relative to the field
*/
public static Pose3d estimateFieldToRobotAprilTag(
Transform3d cameraToTarget, Pose3d fieldRelativeTagPose, Transform3d cameraToRobot) {
return fieldRelativeTagPose.plus(cameraToTarget.inverse()).plus(cameraToRobot);
}

/**
* Returns the yaw between your robot and a target.
*
* @param robotPose Current pose of the robot
* @param targetPose Pose of the target on the field
* @return double Yaw to the target
*/
public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) {
Translation2d relativeTrl = targetPose.relativeTo(robotPose).getTranslation();
return new Rotation2d(relativeTrl.getX(), relativeTrl.getY());
}

/**
* Returns the distance between two poses
*
* @param robotPose Pose of the robot
* @param targetPose Pose of the target
* @return
*/
public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) {
return robotPose.getTranslation().getDistance(targetPose.getTranslation());
}
}
24 changes: 21 additions & 3 deletions photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,7 @@

package org.photonvision;

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.*;
import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.Test;
Expand Down Expand Up @@ -81,4 +79,24 @@ public void testTransform() {
Assertions.assertEquals(0, fieldToRobot.getY(), 0.1);
Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1);
}

@Test
public void testAprilTagUtils() {
var cameraToTarget = new Transform3d(new Translation3d(1, 0, 0), new Rotation3d());
var tagPose = new Pose3d(5, 0, 0, new Rotation3d());
var cameraToRobot = new Transform3d();

var fieldToRobot =
PhotonUtils.estimateFieldToRobotAprilTag(cameraToTarget, tagPose, cameraToRobot);

var targetPose =
new Pose2d(
new Translation2d(Units.inchesToMeters(324), Units.inchesToMeters(162)),
new Rotation2d());
var currentPose = new Pose2d(0, 0, Rotation2d.fromDegrees(0));
Assertions.assertEquals(4.0, fieldToRobot.getX());
Assertions.assertEquals(
Math.toDegrees(Math.atan2((Units.inchesToMeters(162)), (Units.inchesToMeters(324)))),
PhotonUtils.getYawToPose(currentPose, targetPose).getDegrees());
}
}
0