Refactor path flipping into FlippingUtil

pathplannerlib-python/pathplannerlib/auto.py

@@ -5,7 +5,7 @@
 from wpimath.geometry import Pose2d
 from wpimath.kinematics import ChassisSpeeds
 from .commands import FollowPathCommand, PathfindingCommand, PathfindThenFollowPath
-from .geometry_util import flipFieldPose
+from .util import FlippingUtil
 from .controller import PathFollowingController
 import os
 from wpilib import getDeployDirectory, reportError, reportWarning, SendableChooser
@@ -322,7 +322,7 @@ def pathfindToPoseFlipped(pose: Pose2d, constraints: PathConstraints, goal_end_v
         :return: A command to pathfind to a given pose
         """
         return cmd.either(
-            AutoBuilder.pathfindToPose(flipFieldPose(pose), constraints, goal_end_vel),
+            AutoBuilder.pathfindToPose(FlippingUtil.flipFieldPose(pose), constraints, goal_end_vel),
             AutoBuilder.pathfindToPose(pose, constraints, goal_end_vel),
             AutoBuilder._shouldFlipPath
         )
@@ -350,7 +350,7 @@ def resetOdom(bluePose: Pose2d) -> Command:
         :return: Command to reset the robot's odometry
         """
         return cmd.runOnce(lambda: AutoBuilder._resetPose(
-            flipFieldPose(bluePose) if AutoBuilder._shouldFlipPath() else bluePose))
+            FlippingUtil.flipFieldPose(bluePose) if AutoBuilder._shouldFlipPath() else bluePose))
 
     @staticmethod
     def buildAuto(auto_name: str) -> Command:

pathplannerlib-python/pathplannerlib/commands.py

@@ -3,7 +3,7 @@
 from .trajectory import PathPlannerTrajectory, DriveFeedforward
 from .telemetry import PPLibTelemetry
 from .logging import PathPlannerLogging
-from .geometry_util import floatLerp, flipFieldPose
+from .util import floatLerp, FlippingUtil
 from wpimath.geometry import Pose2d
 from wpimath.kinematics import ChassisSpeeds
 from wpilib import Timer
@@ -259,7 +259,7 @@ def initialize(self):
             self._originalTargetPose = Pose2d(self._targetPath.getPoint(0).position,
                                               self._originalTargetPose.rotation())
             if self._shouldFlipPath():
-                self._targetPose = flipFieldPose(self._originalTargetPose)
+                self._targetPose = FlippingUtil.flipFieldPose(self._originalTargetPose)
                 self._goalEndState = GoalEndState(self._goalEndState.velocity, self._targetPose.rotation())
 
         if currentPose.translation().distance(self._targetPose.translation()) < 0.5:

pathplannerlib-python/pathplannerlib/path.py

@@ -8,7 +8,7 @@
 import wpimath.units as units
 from wpimath import inputModulus
 from commands2 import Command, cmd
-from .geometry_util import cubicLerp, calculateRadius, flipFieldPos, flipFieldRotation, floatLerp
+from .util import cubicLerp, calculateRadius, floatLerp, FlippingUtil
 from .trajectory import PathPlannerTrajectory, PathPlannerTrajectoryState
 from .config import RobotConfig
 from wpilib import getDeployDirectory
@@ -200,7 +200,7 @@ def flip(self) -> PointTowardsZone:
 
         :return: The flipped zone
         """
-        return PointTowardsZone(flipFieldPos(self.targetPosition), self.minWaypointRelativePos,
+        return PointTowardsZone(FlippingUtil.flipFieldPosition(self.targetPosition), self.minWaypointRelativePos,
                                 self.maxWaypointRelativePos, self.rotationOffset)
 
 
@@ -258,12 +258,12 @@ class PathPoint:
     waypointRelativePos: float = 0.0
 
     def flip(self) -> PathPoint:
-        flipped = PathPoint(flipFieldPos(self.position))
+        flipped = PathPoint(FlippingUtil.flipFieldPosition(self.position))
         flipped.distanceAlongPath = self.distanceAlongPath
         flipped.maxV = self.maxV
         if self.rotationTarget is not None:
             flipped.rotationTarget = RotationTarget(self.rotationTarget.waypointRelativePosition,
-                                                    flipFieldRotation(self.rotationTarget.target))
+                                                    FlippingUtil.flipFieldRotation(self.rotationTarget.target))
         flipped.constraints = self.constraints
         flipped.waypointRelativePos = self.waypointRelativePos
         return flipped
@@ -289,9 +289,9 @@ def flip(self) -> Waypoint:
 
         :return: The flipped waypoint
         """
-        flippedPrevControl = None if self.prevControl is None else flipFieldPos(self.prevControl)
-        flippedAnchor = flipFieldPos(self.anchor)
-        flippedNextControl = None if self.nextControl is None else flipFieldPos(self.nextControl)
+        flippedPrevControl = None if self.prevControl is None else FlippingUtil.flipFieldPosition(self.prevControl)
+        flippedAnchor = FlippingUtil.flipFieldPosition(self.anchor)
+        flippedNextControl = None if self.nextControl is None else FlippingUtil.flipFieldPosition(self.nextControl)
         return Waypoint(flippedPrevControl, flippedAnchor, flippedNextControl)
 
     @staticmethod
@@ -724,23 +724,24 @@ def flipPath(self) -> PathPlannerPath:
             # Flip the ideal trajectory
             flippedTraj = self._idealTrajectory.flip()
 
-        newRotTargets = [RotationTarget(t.waypointRelativePosition, flipFieldRotation(t.target)) for t in
+        newRotTargets = [RotationTarget(t.waypointRelativePosition, FlippingUtil.flipFieldRotation(t.target)) for t in
                          self._rotationTargets]
         newPointZones = [z.flip() for z in self.point_towards_zones]
 
         newPoints = [p.flip() for p in self._allPoints]
 
         path = PathPlannerPath.fromPathPoints(newPoints, self._globalConstraints,
                                               GoalEndState(self._goalEndState.velocity,
-                                                           flipFieldRotation(self._goalEndState.rotation)))
+                                                           FlippingUtil.flipFieldRotation(self._goalEndState.rotation)))
         path._bezierPoints = [w.flip() for w in self._waypoints]
         path._rotationTargets = newRotTargets
         path._pointTowardsZones = newPointZones
         path._constraintZones = self._constraintZones
         path._eventMarkers = self._eventMarkers
         if self._idealStartingState is not None:
             path._idealStartingState = IdealStartingState(self._idealStartingState.velocity,
-                                                          flipFieldRotation(self._idealStartingState.rotation))
+                                                          FlippingUtil.flipFieldRotation(
+                                                              self._idealStartingState.rotation))
         path._reversed = self._reversed
         path._isChoreoPath = self._isChoreoPath
         path._idealTrajectory = flippedTraj

pathplannerlib-python/pathplannerlib/trajectory.py

@@ -4,7 +4,7 @@
 from dataclasses import dataclass, field
 from wpimath.geometry import Translation2d, Rotation2d, Pose2d
 from wpimath.kinematics import ChassisSpeeds, SwerveModuleState
-from .geometry_util import floatLerp, rotationLerp, poseLerp, calculateRadius, flipFieldPose
+from .util import floatLerp, rotationLerp, poseLerp, calculateRadius, FlippingUtil
 from .config import RobotConfig
 from .events import *
 from typing import List, Union, TYPE_CHECKING
@@ -106,27 +106,15 @@ def flip(self) -> PathPlannerTrajectoryState:
 
         :return: This trajectory state flipped to the other side of the field
         """
-        mirrored = PathPlannerTrajectoryState()
-
-        mirrored.timeSeconds = self.timeSeconds
-        mirrored.linearVelocity = self.linearVelocity
-        mirrored.pose = flipFieldPose(self.pose)
-        mirrored.fieldSpeeds = ChassisSpeeds(-self.fieldSpeeds.vx, self.fieldSpeeds.vy,
-                                             -self.fieldSpeeds.omega)
-        if len(self.feedforwards) == 4:
-            mirrored.feedforwards = [
-                self.feedforwards[1],
-                self.feedforwards[0],
-                self.feedforwards[3],
-                self.feedforwards[2],
-            ]
-        elif len(self.feedforwards) == 2:
-            mirrored.feedforwards = [
-                self.feedforwards[1],
-                self.feedforwards[0],
-            ]
-
-        return mirrored
+        flipped = PathPlannerTrajectoryState()
+
+        flipped.timeSeconds = self.timeSeconds
+        flipped.linearVelocity = self.linearVelocity
+        flipped.pose = FlippingUtil.flipFieldPose(self.pose)
+        flipped.fieldSpeeds = FlippingUtil.flipFieldSpeeds(self.fieldSpeeds)
+        flipped.feedforwards = FlippingUtil.flipFeedforwards(self.feedforwards)
+
+        return flipped
 
     def copyWithTime(self, time: float) -> PathPlannerTrajectoryState:
         """

pathplannerlib-python/pathplannerlib/geometry_util.py → pathplannerlib-python/pathplannerlib/util.py

@@ -1,35 +1,85 @@
 from wpimath.geometry import Translation2d, Rotation2d, Pose2d
+from wpimath.kinematics import ChassisSpeeds
+from enum import Enum
 import math
-
-FIELD_LENGTH = 16.54
-
-
-def flipFieldPos(pos: Translation2d) -> Translation2d:
-    """
-    Flip a field position to the other side of the field, maintaining a blue alliance origin
-
-    :param pos: The position to flip
-    :return: The flipped position
-    """
-    return Translation2d(FIELD_LENGTH - pos.X(), pos.Y())
-
-
-def flipFieldRotation(rotation: Rotation2d) -> Rotation2d:
-    """
-    Flip a field rotation to the other side of the field, maintaining a blue alliance origin
-    :param rotation: The rotation to flip
-    :return: The flipped rotation
-    """
-    return Rotation2d.fromDegrees(180) - rotation
-
-
-def flipFieldPose(pose: Pose2d) -> Pose2d:
-    """
-    Flip a field pose to the other side of the field, maintaining a blue alliance origin
-    :param pose: The pose to flip
-    :return: The flipped pose
-    """
-    return Pose2d(flipFieldPos(pose.translation()), flipFieldRotation(pose.rotation()))
+from typing import List, TYPE_CHECKING
+
+if TYPE_CHECKING:
+    from .trajectory import DriveFeedforward
+
+
+class FieldSymmetry(Enum):
+    kRotational = 1
+    kMirrored = 2
+
+
+class FlippingUtil:
+    symmetryType: FieldSymmetry = FieldSymmetry.kMirrored
+    fieldSizeX: float = 16.54175
+    fieldSizeY: float = 8.211
+
+    @staticmethod
+    def flipFieldPosition(pos: Translation2d) -> Translation2d:
+        """
+        Flip a field position to the other side of the field, maintaining a blue alliance origin
+
+        :param pos: The position to flip
+        :return: The flipped position
+        """
+        if FlippingUtil.symmetryType == FieldSymmetry.kMirrored:
+            return Translation2d(FlippingUtil.fieldSizeX - pos.X(), pos.Y())
+        else:
+            return Translation2d(FlippingUtil.fieldSizeX - pos.X(), FlippingUtil.fieldSizeY - pos.Y())
+
+    @staticmethod
+    def flipFieldRotation(rotation: Rotation2d) -> Rotation2d:
+        """
+        Flip a field rotation to the other side of the field, maintaining a blue alliance origin
+
+        :param rotation: The rotation to flip
+        :return: The flipped rotation
+        """
+        return Rotation2d(math.pi) - rotation
+
+    @staticmethod
+    def flipFieldPose(pose: Pose2d) -> Pose2d:
+        """
+        Flip a field pose to the other side of the field, maintaining a blue alliance origin
+
+        :param pose: The pose to flip
+        :return: The flipped pose
+        """
+        return Pose2d(FlippingUtil.flipFieldPosition(pose.translation()),
+                      FlippingUtil.flipFieldRotation(pose.rotation()))
+
+    @staticmethod
+    def flipFieldSpeeds(fieldSpeeds: ChassisSpeeds) -> ChassisSpeeds:
+        """
+        Flip field relative chassis speeds for the other side of the field, maintaining a blue alliance origin
+
+        :param fieldSpeeds: Field relative chassis speeds
+        :return: Flipped speeds
+        """
+        if FlippingUtil.symmetryType == FieldSymmetry.kMirrored:
+            return ChassisSpeeds(-fieldSpeeds.vx, fieldSpeeds.vy, -fieldSpeeds.omega)
+        else:
+            return ChassisSpeeds(-fieldSpeeds.vx, -fieldSpeeds.vy, fieldSpeeds.omega)
+
+    @staticmethod
+    def flipFeedforwards(feedforwards: List['DriveFeedforward']) -> List['DriveFeedforward']:
+        """
+        Flip a list of drive feedforwards for the other side of the field.
+        Only does anything if mirrored symmetry is used
+
+        :param feedforwards: List of drive feedforwards
+        :return: The flipped feedforwards
+        """
+        if FlippingUtil.symmetryType == FieldSymmetry.kMirrored:
+            if len(feedforwards) == 4:
+                return [feedforwards[1], feedforwards[0], feedforwards[3], feedforwards[2]]
+            elif len(feedforwards) == 2:
+                return [feedforwards[1], feedforwards[0]]
+        return feedforwards
 
 
 def floatLerp(start_val: float, end_val: float, t: float) -> float:

pathplannerlib-python/tests/geometry_util_test.py → pathplannerlib-python/tests/util_test.py

@@ -1,4 +1,4 @@
-from pathplannerlib.geometry_util import *
+from pathplannerlib.util import *
 
 def test_floatLerp():
     assert floatLerp(1.0, 2.0, 0.5) == 1.5

pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java

@@ -6,7 +6,7 @@
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.DriveFeedforward;
-import com.pathplanner.lib.util.GeometryUtil;
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -274,7 +274,7 @@ public static Command pathfindToPose(Pose2d pose, PathConstraints constraints) {
   public static Command pathfindToPoseFlipped(
       Pose2d pose, PathConstraints constraints, double goalEndVelocity) {
     return Commands.either(
-        pathfindToPose(GeometryUtil.flipFieldPose(pose), constraints, goalEndVelocity),
+        pathfindToPose(FlippingUtil.flipFieldPose(pose), constraints, goalEndVelocity),
         pathfindToPose(pose, constraints, goalEndVelocity),
         shouldFlipPath);
   }
@@ -451,7 +451,7 @@ public static Command resetOdom(Pose2d bluePose) {
         () -> {
           boolean flip = shouldFlipPath.getAsBoolean();
           if (flip) {
-            resetPose.accept(GeometryUtil.flipFieldPose(bluePose));
+            resetPose.accept(FlippingUtil.flipFieldPose(bluePose));
           } else {
             resetPose.accept(bluePose);
           }

pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java

@@ -214,7 +214,7 @@ public void initialize() {
       originalTargetPose =
           new Pose2d(this.targetPath.getPoint(0).position, originalTargetPose.getRotation());
       if (shouldFlipPath.getAsBoolean()) {
-        targetPose = GeometryUtil.flipFieldPose(this.originalTargetPose);
+        targetPose = FlippingUtil.flipFieldPose(this.originalTargetPose);
         goalEndState = new GoalEndState(goalEndState.velocity(), targetPose.getRotation());
       }
     }

pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java

@@ -1,6 +1,6 @@
 package com.pathplanner.lib.path;
 
-import com.pathplanner.lib.util.GeometryUtil;
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.json.simple.JSONObject;
 
@@ -29,6 +29,6 @@ static GoalEndState fromJson(JSONObject endStateJson) {
    * @return The flipped end state
    */
   public GoalEndState flip() {
-    return new GoalEndState(velocity, GeometryUtil.flipFieldRotation(rotation));
+    return new GoalEndState(velocity, FlippingUtil.flipFieldRotation(rotation));
   }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/path/IdealStartingState.java

@@ -1,6 +1,6 @@
 package com.pathplanner.lib.path;
 
-import com.pathplanner.lib.util.GeometryUtil;
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.json.simple.JSONObject;
 
@@ -30,6 +30,6 @@ static IdealStartingState fromJson(JSONObject startingStateJson) {
    * @return The flipped starting state
    */
   public IdealStartingState flip() {
-    return new IdealStartingState(velocity, GeometryUtil.flipFieldRotation(rotation));
+    return new IdealStartingState(velocity, FlippingUtil.flipFieldRotation(rotation));
   }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java

@@ -1,6 +1,6 @@
 package com.pathplanner.lib.path;
 
-import com.pathplanner.lib.util.GeometryUtil;
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import java.util.Objects;
 
@@ -60,13 +60,11 @@ public PathPoint(Translation2d position) {
    * @return The flipped point
    */
   public PathPoint flip() {
-    PathPoint flipped = new PathPoint(GeometryUtil.flipFieldPosition(position));
+    PathPoint flipped = new PathPoint(FlippingUtil.flipFieldPosition(position));
     flipped.distanceAlongPath = distanceAlongPath;
     flipped.maxV = maxV;
     if (rotationTarget != null) {
-      flipped.rotationTarget =
-          new RotationTarget(
-              rotationTarget.position(), GeometryUtil.flipFieldRotation(rotationTarget.rotation()));
+      flipped.rotationTarget = rotationTarget.flip();
     }
     flipped.constraints = constraints;
     flipped.waypointRelativePos = waypointRelativePos;

pathplannerlib/src/main/java/com/pathplanner/lib/path/PointTowardsZone.java

@@ -1,6 +1,6 @@
 package com.pathplanner.lib.path;
 
-import com.pathplanner.lib.util.GeometryUtil;
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import org.json.simple.JSONObject;
@@ -61,6 +61,6 @@ private static Translation2d translationFromJson(JSONObject translationJson) {
    */
   public PointTowardsZone flip() {
     return new PointTowardsZone(
-        GeometryUtil.flipFieldPosition(targetPosition), rotationOffset, minPosition, maxPosition);
+        FlippingUtil.flipFieldPosition(targetPosition), rotationOffset, minPosition, maxPosition);
   }
 }