fix(cmd_vel): add hysteresis to deadzone snap and trajectory gates#181
Draft
xiaolefang-dm wants to merge 3 commits into
Draft
fix(cmd_vel): add hysteresis to deadzone snap and trajectory gates#181xiaolefang-dm wants to merge 3 commits into
xiaolefang-dm wants to merge 3 commits into
Conversation
added 3 commits
July 2, 2026 15:31
The min_effective_linear/angular_speed snap-to-min-or-zero logic had a single threshold, so a planner target hovering near min_effective_* caused the executed command to bang-bang between 0 and min. Add engage/release hysteresis (reusing the previously-unused linear_engage_threshold, plus a new angular_engage_threshold) so once moving, the robot stays engaged until the target genuinely drops below the lower threshold.
front_clearance <= 0.30m flipped the reverse-vs-forward trajectory pool with a single threshold, causing the gate to oscillate every cycle when the robot idled right at the boundary. Add a wider exit threshold (0.40m) so once reverse mode engages, it only releases once clearance genuinely improves.
The rotate-first gate (forward motion blocked, in-place turn only) used a single 0.45 rad heading-error threshold, so it flip-flopped every cycle when the planner's selected heading jittered near the boundary (common near obstacles). Add a lower exit threshold (15 deg) so once rotate-only mode engages, forward motion only resumes once heading error genuinely drops.
|
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
Add hysteresis (dual enter/exit thresholds) to three bang-bang-prone gates in the velocity pipeline:
1. Linear/angular deadzone snap (
cmd_vel_control.py)min_effective_*_speedthreshold — when planner target hovered near the boundary, cmd_vel snapped between 0 and min every cycle.min_effective_*_speed; once engaged, stay engaged until target drops below a lower*_engage_threshold(0.04). Eliminates bang-bang on noisy targets.2. Forward/reverse trajectory gate (
planning_node.py)enter_threshold = 0.30— gate flipped every cycle when front clearance hovered at the boundary.0.30, require0.40clearance to exit. Hysteresis state stored inself._reverse_mode.3. Rotate-first heading gate (
cmd_vel_control.py)0.45 radthreshold — flip-flopped between drive+turn and rotate-only when heading jittered near boundary.rotate_first_enter_threshold = 0.45 rad(~26°), stay engaged until heading error drops belowrotate_first_exit_threshold = 15°.All hysteresis states are reset on pause, nav deactivation, and reverse command for clean transitions.
Changed files
tinynav/core/planning_node.py— +12 -1tinynav/platforms/cmd_vel_control.py— +57 -10Testing