Is there a builtin function to find the angle needed to get one Pose2d to face another Pose2d

[Joel_Singh]

Hi, I’m implementing a command that takes the current robot position and rotates it to face the speaker or another field element. I’m wondering if there’s a builtin method to find the angle we need to rotate.

[Knufire]

Not in Pose2D, but Translation2D has some common vector operations that will help: Transformations — FIRST Robotics Competition documentation

[Joel_Singh]

Thanks!

For anybody else who stumbles upon this thread in the future, here’s the code:

        Translation2d pointToFace = /* whatever */;
        Rotation2d rotationNeeded = pointToFace.minus(currentPose.getTranslation()).getAngle();
        return driveToRotation(rotationNeeded.getRadians());