[wpimath] Alliance Coordinate Symmetry Utility

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

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import java.util.HashMap;
+
+/**
+ * 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 AllianceSymetry {
+  private AllianceSymetry() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /** The strategy to use for flipping coordinates over axis of symetry. */
+  public static enum SymetryStrategy {
+    /**
+     * X becomes fieldLength - x, leaves the y coordinate unchanged, and heading becomes PI -
+     * heading.
+     */
+    VERTICAL {
+      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. */
+    ROTATIONAL {
+      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;
+      }
+    },
+    /** Leaves the X coordinate unchanged, Y becomes fieldWidth - y, and heading becomes PI - heading. */
+    HORIZONTAL {
+      public double flipX(double x) {
+        return 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(SymetryStrategy strategy);
+
+    /** Flips the object based on the active flipper. */
+    public default Self flip() {
+      return flip(getFlipper());
+    }
+  }
+
+  private static record YearInfo(SymetryStrategy flipper, double fieldLength, double fieldWidth) {}
+
+  private static final HashMap<Integer, YearInfo> flipperMap =
+      new HashMap<Integer, YearInfo>() {
+        {
+          put(2022, new YearInfo(SymetryStrategy.ROTATIONAL, 16.4592, 8.2296));
+          put(2023, new YearInfo(SymetryStrategy.VERTICAL, 16.54175, 8.0137));
+          put(2024, new YearInfo(SymetryStrategy.VERTICAL, 16.54175, 8.211));
+        }
+      };
+
+  private static YearInfo activeYear = flipperMap.get(2024);
+
+  /**
+   * 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 SymetryStrategy getFlipper() {
+    return activeYear.flipper;
+  }
+
+  /**
+   * 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);
+  }
+
+  /**
+   * 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();
+  }
+}

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.AllianceSymetry.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -24,7 +25,8 @@
 /** 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 +350,14 @@ public Pose2d interpolate(Pose2d endValue, double t) {
     }
   }
 
+  @Override
+  public Pose2d flip(SymetryStrategy strategy) {
+    return new Pose2d(
+        m_translation.flip(strategy),
+        m_rotation.flip(strategy)
+    );
+  }
+
   /** 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.AllianceSymetry.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.jni.Pose3dJNI;
 import edu.wpi.first.units.measure.Distance;
@@ -22,7 +23,8 @@
 /** 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 +373,11 @@ public Pose3d interpolate(Pose3d endValue, double t) {
     }
   }
 
+  @Override
+  public Pose3d flip(SymetryStrategy strategy) {
+    return new Pose3d(m_translation.flip(strategy), m_rotation.flip(strategy));
+  }
+
   /** 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.AllianceSymetry.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
@@ -31,7 +32,10 @@
 @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 +336,15 @@ public Rotation2d interpolate(Rotation2d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 
+  @Override
+  public Rotation2d flip(SymetryStrategy strategy) {
+    return switch (strategy) {
+      case VERTICAL -> new Rotation2d(-getCos(), getSin());
+      case ROTATIONAL -> new Rotation2d(-getCos(), -getSin());
+      case HORIZONTAL -> 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.AllianceSymetry.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.Angle;
@@ -30,7 +31,10 @@
 @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 +508,16 @@ public Rotation3d interpolate(Rotation3d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 
+  @Override
+  public Rotation3d flip(SymetryStrategy strategy) {
+      Rotation2d rot2d = toRotation2d().flip(strategy);
+      return new Rotation3d(
+          this.getX(),
+          this.getY(),
+          rot2d.getRadians()
+      );
+  }
+
   /** Rotation3d protobuf for serialization. */
   public static final Rotation3dProto proto = new Rotation3dProto();
 

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

@@ -15,6 +15,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Translation2dProto;
 import edu.wpi.first.math.geometry.struct.Translation2dStruct;
+import edu.wpi.first.math.geometry.AllianceSymetry.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.units.measure.Distance;
@@ -34,7 +35,10 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Translation2d
-    implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable {
+    implements Interpolatable<Translation2d>,
+        ProtobufSerializable,
+        StructSerializable,
+        Flippable<Translation2d> {
   /**
    * A preallocated Translation2d representing the origin.
    *
@@ -312,6 +316,11 @@ public Translation2d interpolate(Translation2d endValue, double t) {
         MathUtil.interpolate(this.getY(), endValue.getY(), t));
   }
 
+  @Override
+  public Translation2d flip(SymetryStrategy strategy) {
+    return new Translation2d(strategy.flipX(m_x), strategy.flipY(m_y));
+  }
+
   /** Translation2d protobuf for serialization. */
   public static final Translation2dProto proto = new Translation2dProto();
 

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

@@ -15,6 +15,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Translation3dProto;
 import edu.wpi.first.math.geometry.struct.Translation3dStruct;
+import edu.wpi.first.math.geometry.AllianceSymetry.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.Distance;
@@ -32,7 +33,10 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Translation3d
-    implements Interpolatable<Translation3d>, ProtobufSerializable, StructSerializable {
+    implements Interpolatable<Translation3d>,
+        ProtobufSerializable,
+        StructSerializable,
+        Flippable<Translation3d> {
   /**
    * A preallocated Translation3d representing the origin.
    *
@@ -306,6 +310,15 @@ public Translation3d interpolate(Translation3d endValue, double t) {
         MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
   }
 
+  @Override
+  public Translation3d flip(SymetryStrategy strategy) {
+    return new Translation3d(
+      strategy.flipX(m_x),
+      strategy.flipY(m_y),
+      m_z
+    );
+  }
+
   /** Translation3d protobuf for serialization. */
   public static final Translation3dProto proto = new Translation3dProto();