Fix FlippingUtil behavior with kMirrored robot-relative forces and kRotational field rotation

pathplannerlib-python/pathplannerlib/util.py

@@ -77,8 +77,8 @@ def flip(self) -> 'DriveFeedforwards':
             FlippingUtil.flipFeedforwards(self.accelerationsMPS),
             FlippingUtil.flipFeedforwards(self.forcesNewtons),
             FlippingUtil.flipFeedforwards(self.torqueCurrentsAmps),
-            FlippingUtil.flipFeedforwards(self.robotRelativeForcesXNewtons),
-            FlippingUtil.flipFeedforwards(self.robotRelativeForcesYNewtons)
+            FlippingUtil.flipFeedforwardXs(self.robotRelativeForcesXNewtons),
+            FlippingUtil.flipFeedforwardYs(self.robotRelativeForcesYNewtons)
         )
 
     @staticmethod
@@ -112,7 +112,10 @@ def flipFieldRotation(rotation: Rotation2d) -> Rotation2d:
         :param rotation: The rotation to flip
         :return: The flipped rotation
         """
-        return Rotation2d(math.pi) - rotation
+        if FlippingUtil.symmetryType == FieldSymmetry.kMirrored:
+            return Rotation2d(math.pi) - rotation
+        else:
+            return rotation - Rotation2d(math.pi)
 
     @staticmethod
     def flipFieldPose(pose: Pose2d) -> Pose2d:
@@ -154,6 +157,31 @@ def flipFeedforwards(feedforwards: List[float]) -> List[float]:
                 return [feedforwards[1], feedforwards[0]]
         return feedforwards
 
+    @staticmethod
+    def flipFeedforwardXs(feedforwardXs: List[float]) -> List[float]:
+        """
+        Flip a list of drive feedforward X components for the other side of
+        the field. Only does anything if mirrored symmetry is used
+
+        :param feedforwardXs: List of drive feedforward X components
+        :return: The flipped feedforward X components
+        """
+        return FlippingUtil.flipFeedforwards(feedforwardXs)
+
+    @staticmethod
+    def flipFeedforwardYs(feedforwardYs: List[float]) -> List[float]:
+        """
+        Flip a list of drive feedforward Y components for the other side of
+        the field. Only does anything if mirrored symmetry is used
+
+        :param feedforwardYs: List of drive feedforward Y components
+        :return: The flipped feedforward Y components
+        """
+        flippedFeedforwardYs = FlippingUtil.flipFeedforwards(feedforwardYs)
+        if FlippingUtil.symmetryType == FieldSymmetry.kMirrored:
+            return [-feedforward for feedforward in flippedFeedforwardYs]
+        return flippedFeedforwardYs
+
 
 def floatLerp(start_val: float, end_val: float, t: float) -> float:
     """

pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java

@@ -115,8 +115,8 @@ public DriveFeedforwards flip() {
         FlippingUtil.flipFeedforwards(accelerationsMPSSq),
         FlippingUtil.flipFeedforwards(linearForcesNewtons),
         FlippingUtil.flipFeedforwards(torqueCurrentsAmps),
-        FlippingUtil.flipFeedforwards(robotRelativeForcesXNewtons),
-        FlippingUtil.flipFeedforwards(robotRelativeForcesYNewtons));
+        FlippingUtil.flipFeedforwardXs(robotRelativeForcesXNewtons),
+        FlippingUtil.flipFeedforwardYs(robotRelativeForcesYNewtons));
   }
 
   /**

pathplannerlib/src/main/java/com/pathplanner/lib/util/FlippingUtil.java

@@ -46,7 +46,8 @@ public static Translation2d flipFieldPosition(Translation2d pos) {
    */
   public static Rotation2d flipFieldRotation(Rotation2d rotation) {
     return switch (symmetryType) {
-      case kMirrored, kRotational -> new Rotation2d(Math.PI).minus(rotation);
+      case kMirrored -> new Rotation2d(Math.PI).minus(rotation);
+      case kRotational -> rotation.minus(new Rotation2d(Math.PI));
     };
   }
 
@@ -101,4 +102,36 @@ public static double[] flipFeedforwards(double[] feedforwards) {
       case kRotational -> feedforwards;
     };
   }
+
+  /**
+   * Flip an array of drive feedforward X components for the other side of the field. Only does
+   * anything if mirrored symmetry is used
+   *
+   * @param feedforwardXs Array of drive feedforward X components
+   * @return The flipped feedforward X components
+   */
+  public static double[] flipFeedforwardXs(double[] feedforwardXs) {
+    return flipFeedforwards(feedforwardXs);
+  }
+
+  /**
+   * Flip an array of drive feedforward Y components for the other side of the field. Only does
+   * anything if mirrored symmetry is used
+   *
+   * @param feedforwardYs Array of drive feedforward Y components
+   * @return The flipped feedforward Y components
+   */
+  public static double[] flipFeedforwardYs(double[] feedforwardYs) {
+    var flippedFeedforwardYs = flipFeedforwards(feedforwardYs);
+    return switch (symmetryType) {
+      case kMirrored -> {
+        // Y directions also need to be inverted
+        for (int i = 0; i < flippedFeedforwardYs.length; ++i) {
+          flippedFeedforwardYs[i] *= -1;
+        }
+        yield flippedFeedforwardYs;
+      }
+      case kRotational -> flippedFeedforwardYs;
+    };
+  }
 }

pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforwards.h

@@ -76,8 +76,8 @@ struct DriveFeedforwards {
 		return DriveFeedforwards { FlippingUtil::flipFeedforwards(
 				accelerations), FlippingUtil::flipFeedforwards(linearForces),
 				FlippingUtil::flipFeedforwards(torqueCurrents),
-				FlippingUtil::flipFeedforwards(robotRelativeForcesX),
-				FlippingUtil::flipFeedforwards(robotRelativeForcesY) };
+				FlippingUtil::flipFeedforwardXs(robotRelativeForcesX),
+				FlippingUtil::flipFeedforwardYs(robotRelativeForcesY) };
 	}
 
 private:

pathplannerlib/src/main/native/include/pathplanner/lib/util/FlippingUtil.h

@@ -44,8 +44,9 @@ class FlippingUtil {
 	static inline frc::Rotation2d flipFieldRotation(
 			const frc::Rotation2d &rotation) {
 		switch (symmetryType) {
-		case kMirrored:
 		case kRotational:
+			return rotation - frc::Rotation2d(180_deg);
+		case kMirrored:
 		default:
 			return frc::Rotation2d(180_deg) - rotation;
 		}
@@ -82,6 +83,13 @@ class FlippingUtil {
 		}
 	}
 
+	/**
+	 * Flip an array of drive feedforwards for the other side of the field. Only does anything if
+	 * mirrored symmetry is used
+	 *
+	 * @param feedforwards Array of drive feedforwards
+	 * @return The flipped feedforwards
+	 */
 	template<class UnitType, class = std::enable_if_t<
 			units::traits::is_unit_t<UnitType>::value>>
 	static inline std::vector<UnitType> flipFeedforwards(
@@ -100,5 +108,44 @@ class FlippingUtil {
 			return feedforwards; // idk
 		}
 	}
+
+	/**
+	 * Flip an array of drive feedforward X components for the other side of the field. Only does
+	 * anything if mirrored symmetry is used
+	 *
+	 * @param feedforwardXs Array of drive feedforward X components
+	 * @return The flipped feedforward X components
+	 */
+	template<class UnitType, class = std::enable_if_t<
+			units::traits::is_unit_t<UnitType>::value>>
+	static inline std::vector<UnitType> flipFeedforwardXs(
+			const std::vector<UnitType> &feedforwardXs) {
+		return flipFeedforwards(feedforwardXs);
+	}
+
+	/**
+	 * Flip an array of drive feedforward Y components for the other side of the field. Only does
+	 * anything if mirrored symmetry is used
+	 *
+	 * @param feedforwardYs Array of drive feedforward Y components
+	 * @return The flipped feedforward Y components
+	 */
+	template<class UnitType, class = std::enable_if_t<
+			units::traits::is_unit_t<UnitType>::value>>
+	static inline std::vector<UnitType> flipFeedforwardYs(
+			const std::vector<UnitType> &feedforwardYs) {
+		auto flippedFeedforwardYs = flipFeedforwards(feedforwardYs);
+		switch (symmetryType) {
+		case kRotational:
+			return flippedFeedforwardYs;
+		case kMirrored:
+		default:
+			// Y directions also need to be inverted
+			for (auto &feedforwardY : flippedFeedforwardYs) {
+				feedforwardY *= -1;
+			}
+			return flippedFeedforwardYs;
+		}
+	}
 };
 }