Skip to content

Update simposeest to use 3d #524

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 4 commits into from
Oct 26, 2022
Merged

Update simposeest to use 3d #524

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
f155164
Update simposeest to use 3d
agasser Oct 21, 2022
1eb309f
Resolve lint issues
agasser Oct 21, 2022
3b7d1d2
Merge remote-tracking branch 'upstream/master' into 516
agasser Oct 24, 2022
b6184f5
Target z rotation is 180
agasser Oct 24, 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
28 changes: 17 additions & 11 deletions photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@

package org.photonlib.examples.simposeest.robot;

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.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import org.photonvision.SimVisionTarget;

Expand Down Expand Up @@ -71,10 +71,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
Expand All @@ -94,9 +94,15 @@ 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, Units.degreesToRadians(180)));

Copy link
Contributor

@mcm001 mcm001 Oct 21, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm pretty sure that the z angle should be 180 degrees, right?

Copy link
Contributor Author

@agasser agasser Oct 24, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, that sounds right. Sorry for the delay, I wanted to run the latest photonvision server to confirm since these rotations have changed since beta-3.

public static final SimVisionTarget kFarTarget =
new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, targetWidth, targetHeight);
new SimVisionTarget(
kFarTargetPose.toPose2d(), targetHeightAboveGround, targetWidth, targetHeight);
}
14 changes: 4 additions & 10 deletions ...amples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,6 @@
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.Transform3d;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
Expand Down Expand Up @@ -86,15 +84,11 @@ public void update(

var res = cam.getLatestResult();
if (res.hasTargets()) {
double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0;
Transform3d camToTargetTrans = res.getBestTarget().getBestCameraToTarget();
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().getBestCameraToTarget();
var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());
m_poseEstimator.addVisionMeasurement(
camPose.transformBy(Constants.kCameraToRobot), imageCaptureTime);
camPose.transformBy(Constants.kCameraToRobot).toPose2d(), imageCaptureTime);
}
}

Expand Down
4 changes: 3 additions & 1 deletion ...nlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down