[wpimath] Alliance Coordinate Symmetry Utility

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.MatchInfoData;
+import edu.wpi.first.math.geometry.AllianceFlipper;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -29,6 +30,13 @@
 
 /** Provide access to the network communication data to / from the Driver Station. */
 public final class DriverStation {
+  static {
+    // i don't like this, 110% open to suggestions
+    AllianceFlipper.setOnRed(
+      () -> getAlliance().orElse(Alliance.Blue) == Alliance.Red
+    );
+  }
+
   /** Number of Joystick ports. */
   public static final int kJoystickPorts = 6;
 

wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java

@@ -0,0 +1,223 @@
+package edu.wpi.first.math.geometry;
+
+import java.util.HashMap;
+import java.util.function.BooleanSupplier;
+
+/**
+ * A utility to standardize flipping of coordinate data based on the current alliance across
+ * different years.
+ *
+ * <p>If every vendor used this, the user would be able to specify the year and no matter the year
+ * the vendor's code is from, the user would be able to flip as expected.
+ */
+public class AllianceFlipper {
+  private AllianceFlipper() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /** The flipper to use for flipping coordinates. */
+  public static enum Flipper {
+    /**
+     * X becomes fieldLength - x, leaves the y coordinate unchanged, and heading becomes PI -
+     * heading.
+     */
+    VERTICALLY_MIRRORED {
+      public double flipX(double x) {
+        return activeYear.fieldLength - x;
+      }
+
+      public double flipY(double y) {
+        return y;
+      }
+
+      public double flipHeading(double heading) {
+        return Math.PI - heading;
+      }
+    },
+    /** X becomes fieldLength - x, Y becomes fieldWidth - y, and heading becomes PI - heading. */
+    ROTATIONALLY_MIRRORED {
+      public double flipX(double x) {
+        return activeYear.fieldLength - x;
+      }
+
+      public double flipY(double y) {
+        return activeYear.fieldWidth - y;
+      }
+
+      public double flipHeading(double heading) {
+        return Math.PI - heading;
+      }
+    };
+
+    /**
+     * Flips the X coordinate.
+     *
+     * @param x The X coordinate to flip.
+     * @return The flipped X coordinate.
+     */
+    public abstract double flipX(double x);
+
+    /**
+     * Flips the Y coordinate.
+     *
+     * @param y The Y coordinate to flip.
+     * @return The flipped Y coordinate.
+     */
+    public abstract double flipY(double y);
+
+    /**
+     * Flips the heading.
+     *
+     * @param heading The heading to flip.
+     * @return The flipped heading.
+     */
+    public abstract double flipHeading(double heading);
+  }
+
+  /**
+   * An interface for objects that can be flipped based on the current alliance.
+   */
+  public static interface Flippable<Self extends Flippable<Self>> {
+    /**
+     * Flips the object based on the supplied flipper.
+     */
+    public Self flip(Flipper flipper);
+
+    /**
+     * Flips the object based on the active flipper.
+     */
+    public default Self flip() {
+      return flip(getFlipper());
+    }
+
+    /**
+     * Flips the object if on the red alliance, otherwise returns the object unchanged.
+     */
+    @SuppressWarnings("unchecked")
+    public default Self flipIfRed() {
+        return onRed() ? flip() : (Self) this;
+    }
+  }
+
+  private static record YearInfo(Flipper flipper, double fieldLength, double fieldWidth) {}
+
+  private static final HashMap<Integer, YearInfo> flipperMap =
+      new HashMap<Integer, YearInfo>() {
+        {
+          put(2022, new YearInfo(Flipper.ROTATIONALLY_MIRRORED, 16.4592, 8.2296));
+          put(2023, new YearInfo(Flipper.VERTICALLY_MIRRORED, 16.54175, 8.0137));
+          put(2024, new YearInfo(Flipper.VERTICALLY_MIRRORED, 16.54175, 8.211));
+        }
+      };
+
+  private static YearInfo activeYear = flipperMap.get(2024);
+  private static BooleanSupplier onRed = () -> false;
+
+  /**
+   * Get the flipper that is currently active for flipping coordinates. It's reccomended not to
+   * store this locally as the flipper may change.
+   *
+   * @return The active flipper.
+   */
+  public static Flipper getFlipper() {
+    return activeYear.flipper;
+  }
+
+  /**
+   * Returns if you are on red alliance, if the alliance is unknown it will return false.
+   *
+   * @return If you are on red alliance.
+   */
+  public static boolean onRed() {
+    return onRed.getAsBoolean();
+  }
+
+  /**
+   * Returns if you are on blue alliance, if the alliance is unknown it will return true.
+   *
+   * @return If you are on blue alliance.
+   */
+  public static boolean onBlue() {
+    return !onRed.getAsBoolean();
+  }
+
+  /**
+   * Set the year to determine the Alliance Coordinate Flipper to use.
+   *
+   * @param year The year to set the flipper to. [2022 - 2024]
+   */
+  public static void setYear(int year) {
+    if (!flipperMap.containsKey(year)) {
+      // Throw an exception instead of just reporting an error
+      // because not flipping correctly during an auto routine
+      // could cause a robot to damage itself or others.
+      throw new IllegalArgumentException("Year " + year + " is not supported.");
+    }
+    activeYear = flipperMap.get(year);
+  }
+
+  /**
+   * Set the `onRed` resolver to determine if the robot is on the red alliance.
+   * 
+   * @param onRedResolver The resolver to determine if the robot is on the red alliance.
+   */
+  public static void setOnRed(BooleanSupplier onRedResolver) {
+    // cannot access driverstation in wpimath
+    onRed = onRedResolver;
+  }
+
+  /**
+   * Flips the X coordinate.
+   *
+   * @param x The X coordinate to flip.
+   * @return The flipped X coordinate.
+   */
+  public static double flipX(double x) {
+    return activeYear.flipper.flipX(x);
+  }
+
+  /**
+   * Flips the Y coordinate.
+   *
+   * @param y The Y coordinate to flip.
+   * @return The flipped Y coordinate.
+   */
+  public static double flipY(double y) {
+    return activeYear.flipper.flipY(y);
+  }
+
+  /**
+   * Flips the heading.
+   *
+   * @param heading The heading to flip.
+   * @return The flipped heading.
+   */
+  public static double flipHeading(double heading) {
+    return activeYear.flipper.flipHeading(heading);
+  }
+
+  /**
+   * Flips the {@link Flippable} object.
+   *
+   * @param <T> The type of the object to flip.
+   * @param flippable The object to flip.
+   * @return The flipped object.
+   */
+  public static <T extends Flippable<T>> T flip(Flippable<T> flippable) {
+    return flippable.flip();
+  }
+
+  /**
+   * Flips the {@link Flippable} object if on the red alliance.
+   *
+   * @param <T> The type of the object to flip.
+   * @param flippable The object to flip.
+   * @return The flipped object.
+   * 
+   * @see #onRed() A way of determining if you are on the red alliance.
+   * @see Flippable#flipIfRed() An instance method that does the same thing.
+   */
+  public static <T extends Flippable<T>> T flipIfRed(Flippable<T> flippable) {
+    return flippable.flipIfRed();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -12,6 +12,7 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.geometry.proto.Pose2dProto;
 import edu.wpi.first.math.geometry.struct.Pose2dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -24,7 +25,7 @@
 /** Represents a 2D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable {
+public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable, Flippable<Pose2d> {
   /**
    * A preallocated Pose2d representing the origin.
    *
@@ -348,6 +349,14 @@ public Pose2d interpolate(Pose2d endValue, double t) {
     }
   }
 
+  @Override
+  public Pose2d flip(Flipper flipper) {
+    return new Pose2d(
+        m_translation.flip(flipper),
+        m_rotation.flip(flipper)
+    );
+  }
+
   /** Pose2d protobuf for serialization. */
   public static final Pose2dProto proto = new Pose2dProto();
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -12,6 +12,7 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.geometry.proto.Pose3dProto;
 import edu.wpi.first.math.geometry.struct.Pose3dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.jni.Pose3dJNI;
 import edu.wpi.first.units.measure.Distance;
@@ -22,7 +23,7 @@
 /** Represents a 3D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable {
+public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable, Flippable<Pose3d> {
   /**
    * A preallocated Pose3d representing the origin.
    *
@@ -371,6 +372,11 @@ public Pose3d interpolate(Pose3d endValue, double t) {
     }
   }
 
+  @Override
+  public Pose3d flip(Flipper flipper) {
+    return new Pose3d(m_translation.flip(flipper), m_rotation.flip(flipper));
+  }
+
   /** Pose3d protobuf for serialization. */
   public static final Pose3dProto proto = new Pose3dProto();
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.proto.Rotation2dProto;
 import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
@@ -31,7 +32,7 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation2d
-    implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable {
+    implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable, Flippable<Rotation2d> {
   /**
    * A preallocated Rotation2d representing no rotation.
    *
@@ -332,6 +333,14 @@ public Rotation2d interpolate(Rotation2d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 
+  @Override
+  public Rotation2d flip(Flipper flipper) {
+    return switch (flipper) {
+      case VERTICALLY_MIRRORED -> new Rotation2d(-getCos(), getSin());
+      case ROTATIONALLY_MIRRORED -> new Rotation2d(-getCos(), -getSin());
+    };
+  }
+
   /** Rotation2d protobuf for serialization. */
   public static final Rotation2dProto proto = new Rotation2dProto();
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -18,6 +18,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Rotation3dProto;
 import edu.wpi.first.math.geometry.struct.Rotation3dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.Angle;
@@ -30,7 +31,7 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d
-    implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable {
+    implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable, Flippable<Rotation3d> {
   /**
    * A preallocated Rotation3d representing no rotation.
    *
@@ -504,6 +505,16 @@ public Rotation3d interpolate(Rotation3d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 
+  @Override
+  public Rotation3d flip(Flipper flipper) {
+      Rotation2d rot2d = toRotation2d().flip(flipper);
+      return new Rotation3d(
+          this.getX(),
+          this.getY(),
+          rot2d.getRadians()
+      );
+  }
+
   /** Rotation3d protobuf for serialization. */
   public static final Rotation3dProto proto = new Rotation3dProto();