diff --git a/src/main/java/lib/Utils.java b/src/main/java/lib/Utils.java
index b85d1425..c0775943 100644
--- a/src/main/java/lib/Utils.java
+++ b/src/main/java/lib/Utils.java
@@ -49,13 +49,15 @@ public static Rotation2d normalize(Rotation2d angle) {
         return Rotation2d.fromRadians(normalize(angle.getRadians()));
     }
 
-    public static double getDistanceFromPoint(Pose2d point, Pose2d robotPose) {
-        return robotPose.getTranslation().getDistance(point.getTranslation());
+    public static double getDistanceFromPoint(Translation2d point, Pose2d robotPose) {
+        return robotPose.getTranslation().getDistance(point);
     }
 
     public static Pose2d calcOptimalPose(List<Pose2d> points, Pose2d robotPose) {
         return points.stream()
-                .min(Comparator.comparingDouble(point -> getDistanceFromPoint(point, robotPose)))
+                .min(
+                        Comparator.comparingDouble(
+                                point -> getDistanceFromPoint(point.getTranslation(), robotPose)))
                 .orElse(null);
     }
 }