From cd108d5238f6ca6a52dac63a17b0bb4aa4fb39f6 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 1 Oct 2024 16:52:05 -0700 Subject: [PATCH] Fix null feedforwards for final trajectory state (#782) --- pathplannerlib-python/pathplannerlib/trajectory.py | 3 +++ .../pathplanner/lib/trajectory/PathPlannerTrajectory.java | 5 +++++ .../pathplanner/lib/trajectory/PathPlannerTrajectory.cpp | 6 ++++++ 3 files changed, 14 insertions(+) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index 8ae241a7..4142fabf 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/pathplannerlib-python/pathplannerlib/trajectory.py @@ -233,6 +233,9 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch self._events.append(ScheduleCommandEvent(prevState.timeSeconds, m.command)) unaddedMarkers.pop(0) + # Create feedforwards for the end state + self._states[-1].feedforwards = [DriveFeedforward(0, 0, 0)] * config.numModules + def getEvents(self) -> List[Event]: """ Get all the events to run while following this trajectory diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java index 8b4bff0b..7df33994 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java @@ -154,6 +154,11 @@ public PathPlannerTrajectory( } } } + + // Create feedforwards for the end state + for (int m = 0; m < config.numModules; m++) { + states.get(states.size() - 1).feedforwards[m] = new DriveFeedforward(0, 0, 0); + } } } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp index 60469d42..f290aa74 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp @@ -120,6 +120,12 @@ PathPlannerTrajectory::PathPlannerTrajectory( unaddedMarkers.erase(unaddedMarkers.begin()); } } + + // Create feedforwards for the end state + for (size_t m = 0; m < config.numModules; m++) { + m_states[m_states.size() - 1].feedforwards.emplace_back( + DriveFeedforward { 0_mps_sq, 0_N, 0_A }); + } } } }