Fix potential division by zero when calculating FF (#1037)

mjansen4857 · Jan 27, 2025 · 4fb3a1c · 4fb3a1c
1 parent 47a765f
commit 4fb3a1c
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 3 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py
@@ -240,7 +240,7 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
                     v0 = prevState.linearVelocity
                     v = state.linearVelocity
                     sumV = v + v0
-                    if abs(sumV) < 1e-6:
+                    if abs(sumV) < 1e-6 or abs(state.deltaPos) < 1e-6:
                         state.timeSeconds = prevState.timeSeconds
                         if i != 1:
                             prevState.feedforwards = self._states[i - 2].feedforwards

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java
@@ -133,7 +133,7 @@ public PathPlannerTrajectory(
         double v0 = prevState.linearVelocity;
         double v = state.linearVelocity;
         double sumV = v + v0;
-        if (Math.abs(sumV) < 1e-6) {
+        if (Math.abs(sumV) < 1e-6 || Math.abs(state.deltaPos) < 1e-6) {
           state.timeSeconds = prevState.timeSeconds;
           if (i != 1) {
             prevState.feedforwards = states.get(i - 2).feedforwards;

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp
@@ -110,7 +110,8 @@ PathPlannerTrajectory::PathPlannerTrajectory(
 			units::meters_per_second_t v0 = prevState.linearVelocity;
 			units::meters_per_second_t v = state.linearVelocity;
 			units::meters_per_second_t sumV = v + v0;
-			if (units::math::abs(sumV) < 1e-6_mps) {
+			if (units::math::abs(sumV) < 1e-6_mps
+					|| units::math::abs(state.deltaPos) < 1e-6_m) {
 				state.time = prevState.time;
 				if (i != 1) {
 					prevState.feedforwards = m_states[i - 2].feedforwards;