feat: add distance-based path follower; fix interpolate bugs#1170
Open
JosephTLockwood wants to merge 3 commits into
Open
feat: add distance-based path follower; fix interpolate bugs#1170JosephTLockwood wants to merge 3 commits into
JosephTLockwood wants to merge 3 commits into
Conversation
Adds: - PathPlannerTrajectoryState.distanceAlongPath: cumulative arc length from the trajectory start, populated automatically in both constructors and preserved through interpolate/flip/reverse/copy. - PathPlannerTrajectory.sampleByDistance(double): query the trajectory by arc length instead of elapsed time. Useful for projection-based path following where progress is tracked from odometry rather than a timer. - PathPlannerTrajectory.getTotalArcLength(): total chord-summed length. Purely additive -- no existing behavior changes. 11 new unit tests cover population, monotonicity, clamping, interpolation, agreement with sample(time), and preservation through flip/copy.
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
Codecov Report✅ All modified and coverable lines are covered by tests. Additional details and impacted files@@ Coverage Diff @@
## main #1170 +/- ##
==========================================
- Coverage 84.59% 84.58% -0.02%
==========================================
Files 95 95
Lines 9750 9750
==========================================
- Hits 8248 8247 -1
- Misses 1502 1503 +1 ☔ View full report in Codecov by Sentry. 🚀 New features to boost your workflow:
|
…gate PPCrossTrackHolonomicController: - Add alongTrackKp parameter to correct tangent-direction lag. The previous controller could only correct perpendicular (cross-track) error; when the robot finished behind the target along the path tangent the velocity FF alone had no recovery mechanism, leaving a residual along-track gap. - Suppress cross-track D-term and curvature FF for any tick where the path tangent direction is rotating faster than 5 rad/s (cusps, U-turns, sharp corners). The cross-track frame rotates with the tangent; at high rates the D-term finite-difference reflects frame rotation rather than real robot motion, and the curvature FF formula breaks down for the same reason. Resumes next tick once the rate drops. - Update defaults() to crossTrackKp=3.0, crossTrackKd=0.5, alongTrackKp=2.0, rotationKp=5.0, curvatureFfGain=0.1. Backward-compatible 3-arg constructor preserved with alongTrackKp=0. FollowPathDistanceCommand: - isFinished velocity gate checks total field-speed magnitude rather than just the tangent component. The tangent-only gate fired while the cross-track PD was still closing perpendicular error, leaving residual lateral offset at path end. 3 new regression tests; 22 total controller+follower tests pass.
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. Depends on #1169 -- until that merges, this PR's diff includes #1169's changes too.
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 #1169'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