Allow use of the PathPlannerTrajectory constructor with choreo paths

mjansen4857 · Jan 30, 2024 · c4c3bb6 · c4c3bb6
1 parent 6abcadb
commit c4c3bb6
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 5 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py
@@ -121,7 +121,10 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
         if states is not None:
             self._states = states
         else:
-            self._states = PathPlannerTrajectory._generateStates(path, starting_speeds, starting_rotation)
+            if path.isChoreoPath():
+                self._states = path.getTrajectory(starting_speeds, starting_rotation).getStates()
+            else:
+                self._states = PathPlannerTrajectory._generateStates(path, starting_speeds, starting_rotation)
 
     def getStates(self) -> List[State]:
         """

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java
@@ -23,7 +23,11 @@ public class PathPlannerTrajectory {
    */
   public PathPlannerTrajectory(
       PathPlannerPath path, ChassisSpeeds startingSpeeds, Rotation2d startingRotation) {
-    this.states = generateStates(path, startingSpeeds, startingRotation);
+    if (path.isChoreoPath()) {
+      this.states = path.getTrajectory(startingSpeeds, startingRotation).getStates();
+    } else {
+      this.states = generateStates(path, startingSpeeds, startingRotation);
+    }
   }
 
   /**

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp
@@ -3,6 +3,18 @@
 
 using namespace pathplanner;
 
+PathPlannerTrajectory::PathPlannerTrajectory(
+		std::shared_ptr<PathPlannerPath> path,
+		const frc::ChassisSpeeds &startingSpeeds,
+		const frc::Rotation2d &startingRotation) {
+	if (path->isChoreoPath()) {
+		m_states =
+				path->getTrajectory(startingSpeeds, startingRotation).getStates();
+	} else {
+		m_states = generateStates(path, startingSpeeds, startingRotation);
+	}
+}
+
 PathPlannerTrajectory::State PathPlannerTrajectory::sample(
 		const units::second_t time) {
 	if (time <= getInitialState().time)

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerTrajectory.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerTrajectory.h
@@ -150,9 +150,7 @@ class PathPlannerTrajectory {
 	 */
 	PathPlannerTrajectory(std::shared_ptr<PathPlannerPath> path,
 			const frc::ChassisSpeeds &startingSpeeds,
-			const frc::Rotation2d &startingRotation) : m_states(
-			generateStates(path, startingSpeeds, startingRotation)) {
-	}
+			const frc::Rotation2d &startingRotation);
 
 	/**
 	 * Get the target state at the given point in time along the trajectory