From 4fb3a1c01755828cb0eb7b98eca1386ad355b79f Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Mon, 27 Jan 2025 11:34:10 -0800 Subject: [PATCH] Fix potential division by zero when calculating FF (#1037) --- pathplannerlib-python/pathplannerlib/trajectory.py | 2 +- .../com/pathplanner/lib/trajectory/PathPlannerTrajectory.java | 2 +- .../cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index be6c0021..024f00e1 100644 --- 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 index fd031954..7f3d3f64 100644 --- 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 index 36e0fb0e..aa61f475 100644 --- 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;