From c4c3bb6e258646fb7ffe866807f34cbca31815da Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 30 Jan 2024 12:53:01 -0500 Subject: [PATCH] Allow use of the PathPlannerTrajectory constructor with choreo paths --- pathplannerlib-python/pathplannerlib/trajectory.py | 5 ++++- .../pathplanner/lib/path/PathPlannerTrajectory.java | 6 +++++- .../pathplanner/lib/path/PathPlannerTrajectory.cpp | 12 ++++++++++++ .../pathplanner/lib/path/PathPlannerTrajectory.h | 4 +--- 4 files changed, 22 insertions(+), 5 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index 920035f2..815ca881 100644 --- 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 index 230909da..b6fd48c6 100644 --- 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 index 1d013c8e..6f6982eb 100644 --- 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 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 index aa30c4ee..d271d346 100644 --- 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 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