feat: add distance-based path follower; fix interpolate bugs#2
Closed
JosephTLockwood wants to merge 1 commit into
Closed
feat: add distance-based path follower; fix interpolate bugs#2JosephTLockwood wants to merge 1 commit into
JosephTLockwood wants to merge 1 commit into
Conversation
Follower stack: - FollowPathDistanceCommand: drives by projected arc length instead of elapsed time. Projects pose onto path via closest-point-on-polyline in a lookahead window, with three guardrails: forward-saturation extension, big-gap fallback to full scan, and max-delta-per-tick. - FollowPathDistanceCommand.EndConditions: nested record holding distance, velocity, and rotation tolerances with defaults() factory. - PPCrossTrackHolonomicController: cross-track PD on perpendicular-to- tangent error (1D), heading PID, and optional curvature feedforward (kappa * v^2) to anticipate centripetal drift on curves. - PathPlannerTrajectoryState.curvatureRadPerMeter: signed path curvature, populated automatically and preserved through interpolate/flip/reverse/copy. PathPlannerTrajectoryState.interpolate fixes (3 pre-existing bugs surfaced by sampleByDistance): - Integration loop started at timeSeconds + 0.01, skipping the first 10 ms step (under-integration on every sample). - For deltaT < 0.01, the break case computed a negative dt, producing backward motion near zero-velocity sections. - For deltaT == 0, intT divided by zero, poisoning the pose with NaN. Integration now starts at timeSeconds, uses remainingTime to gate the final partial step, and skips entirely when deltaT == 0. The heading (chord direction) is intentionally left unlerped during integration -- keeping integrated position on the segment chord and giving callers the correct tangent for perpendicular cross-track measurement. 5 new regression tests; existing tests pass unchanged. 23 new feature tests + 5 regression tests; total 43 tests pass.
3 tasks
Owner
Author
|
Re-targeting upstream against mjansen4857/pathplanner. See replacement PR there. |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Second of three PRs adding distance-based path following. Stacks on top of
feat/sample-by-distance(PR #1, mjansen4857#1169 upstream).Follower stack
FollowPathDistanceCommand-- drives by projected arc length instead of elapsed time. Closest-point-on-polyline projection over a lookahead window of state segments, with three guardrails: forward-saturation extension, big-gap fallback to full scan, and max-delta-per-tick clamp.FollowPathDistanceCommand.EndConditions-- nested record holding distance/velocity/rotation tolerances with adefaults()factory (5cm / 0.1 m/s / 5 deg).PPCrossTrackHolonomicController-- cross-track PD on perpendicular-to-tangent error (1D), heading PID, and optional curvature feedforward (kappa * v^2) to anticipate centripetal drift on curves.PathPlannerTrajectoryState.curvatureRadPerMeter-- signed path curvature, populated automatically and preserved throughinterpolate/flip/reverse/copyWithTime.Pre-existing bug fixes in
PathPlannerTrajectoryState.interpolateThree Euler-integration bugs surfaced by PR #1's
sampleByDistance:timeSeconds + 0.01, skipping the first 10 ms step. Every sample was under-integrated by one step (1cm at 1 m/s).deltaT < 0.01, the break case computed a negativedt, producing backward integration near zero-velocity sections.deltaT == 0,intTdivided by zero, poisoning the pose with NaN.Integration now starts at
timeSeconds, usesremainingTimeto gate the final partial step, and skips entirely whendeltaT == 0. The heading is intentionally left unlerped (chord direction is constant within a segment).Test plan
spotlessJavaCheckclean