diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index 23f36a77..4c1622eb 100644 --- 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 index 9a98bda3..4e0565b4 100644 --- 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 index 2fe32510..e8ea6b7f 100644 --- 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; }