Fix differential drive trajectories

mjansen4857 · Mar 9, 2025 · 91783e3 · 91783e3
1 parent 148181f
commit 91783e3
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 0 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py
@@ -480,6 +480,9 @@ def _generateStates(states: List[PathPlannerTrajectoryState], path: PathPlannerP
         else:
             state.heading = states[i - 1].heading
 
+        if not config.isHolonomic:
+            state.pose = Pose2d(state.pose.translation(), state.heading)
+
         if i != 0:
             state.deltaPos = state.pose.translation().distance(states[i - 1].pose.translation())
             state.deltaRot = state.pose.rotation() - states[i - 1].pose.rotation()

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java
@@ -264,6 +264,10 @@ private static void generateStates(
         state.heading = states.get(i - 1).heading;
       }
 
+      if (!config.isHolonomic) {
+        state.pose = new Pose2d(state.pose.getTranslation(), state.heading);
+      }
+
       if (i != 0) {
         state.deltaPos =
             state.pose.getTranslation().getDistance(states.get(i - 1).pose.getTranslation());

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp
@@ -288,6 +288,10 @@ void PathPlannerTrajectory::generateStates(
 			state.heading = states[i - 1].heading;
 		}
 
+		if (!config.isHolonomic) {
+			state.pose = frc::Pose2d(state.pose.Translation(), state.heading);
+		}
+
 		if (i != 0) {
 			state.deltaPos = state.pose.Translation().Distance(
 					states[i - 1].pose.Translation());