Calculate heading for each state of choreo paths

pathplannerlib-python/pathplannerlib/path.py

@@ -559,6 +559,8 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
                 state.linearVelocity = math.hypot(xVel, yVel)
                 state.pose = Pose2d(xPos, yPos, rotationRad)
                 state.fieldSpeeds = ChassisSpeeds(xVel, yVel, angularVelRps)
+                if abs(state.linearVelocity) > 1e-6:
+                    state.heading = Rotation2d(state.fieldSpeeds.vx, state.fieldSpeeds.vy)
 
                 # The module forces are field relative, rotate them to be robot relative
                 for i in range(len(forcesX)):

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

@@ -389,6 +389,11 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName)
         state.linearVelocity = Math.hypot(xVel, yVel);
         state.pose = new Pose2d(new Translation2d(xPos, yPos), new Rotation2d(rotationRad));
         state.fieldSpeeds = new ChassisSpeeds(xVel, yVel, angularVelRps);
+        if (Math.abs(state.linearVelocity) > 1e-6) {
+          state.heading =
+              new Rotation2d(
+                  state.fieldSpeeds.vxMetersPerSecond, state.fieldSpeeds.vyMetersPerSecond);
+        }
 
         // The module forces are field relative, rotate them to be robot relative
         for (int i = 0; i < forcesX.length; i++) {

pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java

@@ -20,13 +20,13 @@ public class PathPlannerTrajectoryState implements Interpolatable<PathPlannerTra
   public Pose2d pose = Pose2d.kZero;
   /** The linear velocity at this state in m/s */
   public double linearVelocity = 0.0;
+  /** The field-relative heading, or direction of travel, at this state */
+  public Rotation2d heading = Rotation2d.kZero;
 
   /** The feedforwards for each module */
   public DriveFeedforwards feedforwards;
 
   // Values used only during generation, these will not be interpolated
-  /** The field-relative heading, or direction of travel, at this state */
-  protected Rotation2d heading = Rotation2d.kZero;
   /** The distance between this state and the previous state */
   protected double deltaPos = 0.0;
   /** The difference in rotation between this state and the previous state */

pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp

@@ -196,6 +196,10 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache(
 		state.pose = frc::Pose2d(frc::Translation2d(xPos, yPos),
 				frc::Rotation2d(rotationRad));
 		state.fieldSpeeds = frc::ChassisSpeeds { xVel, yVel, angularVelRps };
+		if (units::math::abs(state.linearVelocity) > 1e-6_mps) {
+			state.heading = frc::Rotation2d(state.fieldSpeeds.vx(),
+					state.fieldSpeeds.vy());
+		}
 
 		// The module forces are field relative, rotate them to be robot relative
 		for (size_t i = 0; i < forcesX.size(); i++) {