Fix state interpolation for flipped/reversed trajectories (#946)

mjansen4857 · Dec 20, 2024 · d8f11f1 · d8f11f1
1 parent ddbfaed
commit d8f11f1
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 4 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py
@@ -31,8 +31,8 @@ class PathPlannerTrajectoryState:
     pose: Pose2d = field(default_factory=Pose2d)
     linearVelocity: float = 0.0
     feedforwards: DriveFeedforwards = None
-
     heading: Rotation2d = field(default_factory=Rotation2d)
+
     deltaPos: float = 0.0
     deltaRot: Rotation2d = field(default_factory=Rotation2d)
     moduleStates: List[SwerveModuleTrajectoryState] = field(default_factory=list)
@@ -106,6 +106,7 @@ def reverse(self) -> PathPlannerTrajectoryState:
         reversedState.pose = Pose2d(self.pose.translation(), self.pose.rotation() + Rotation2d.fromDegrees(180))
         reversedState.linearVelocity = -self.linearVelocity
         reversedState.feedforwards = self.feedforwards.reverse()
+        reversedState.heading = self.heading + Rotation2d.fromDegrees(180)
 
         return reversedState
 
@@ -122,6 +123,7 @@ def flip(self) -> PathPlannerTrajectoryState:
         flipped.pose = FlippingUtil.flipFieldPose(self.pose)
         flipped.fieldSpeeds = FlippingUtil.flipFieldSpeeds(self.fieldSpeeds)
         flipped.feedforwards = self.feedforwards.flip()
+        flipped.heading = FlippingUtil.flipFieldRotation(self.heading)
 
         return flipped
 

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java
@@ -112,14 +112,14 @@ public PathPlannerTrajectoryState reverse() {
     reversed.timeSeconds = timeSeconds;
     Translation2d reversedSpeeds =
         new Translation2d(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond)
-            .rotateBy(Rotation2d.fromDegrees(180));
+            .rotateBy(Rotation2d.k180deg);
     reversed.fieldSpeeds =
         new ChassisSpeeds(
             reversedSpeeds.getX(), reversedSpeeds.getY(), fieldSpeeds.omegaRadiansPerSecond);
-    reversed.pose =
-        new Pose2d(pose.getTranslation(), pose.getRotation().plus(Rotation2d.fromDegrees(180)));
+    reversed.pose = new Pose2d(pose.getTranslation(), pose.getRotation().plus(Rotation2d.k180deg));
     reversed.linearVelocity = -linearVelocity;
     reversed.feedforwards = feedforwards.reverse();
+    reversed.heading = heading.plus(Rotation2d.k180deg);
 
     return reversed;
   }
@@ -137,6 +137,7 @@ public PathPlannerTrajectoryState flip() {
     flipped.pose = FlippingUtil.flipFieldPose(pose);
     flipped.fieldSpeeds = FlippingUtil.flipFieldSpeeds(fieldSpeeds);
     flipped.feedforwards = feedforwards.flip();
+    flipped.heading = FlippingUtil.flipFieldRotation(heading);
 
     return flipped;
   }

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp
@@ -72,6 +72,7 @@ PathPlannerTrajectoryState PathPlannerTrajectoryState::reverse() const {
 			pose.Rotation() + frc::Rotation2d(180_deg));
 	reversed.linearVelocity = -linearVelocity;
 	reversed.feedforwards = feedforwards.reverse();
+	reversed.heading = heading + frc::Rotation2d(180_deg);
 
 	return reversed;
 }
@@ -84,6 +85,7 @@ PathPlannerTrajectoryState PathPlannerTrajectoryState::flip() const {
 	flipped.pose = FlippingUtil::flipFieldPose(pose);
 	flipped.fieldSpeeds = FlippingUtil::flipFieldSpeeds(fieldSpeeds);
 	flipped.feedforwards = feedforwards.flip();
+	flipped.heading = FlippingUtil::flipFieldRotation(heading);
 
 	return flipped;
 }