forked from ros-navigation/navigation2
-
Couldn't load subscription status.
- Fork 0
Use last command to initialize state - fix oscillations & nervous steering #4
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
adivardi
wants to merge
35
commits into
enway-devel
Choose a base branch
from
av/mppi_feedback_clean
base: enway-devel
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Conversation
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
leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run
otherwise, the filter may result in control_sequence which violates the hard kinematic and acceleration constraints
This reverts commit fb25b2f.
Color trajectories by costs by adding a red component which is inversely proportional ti the costs of the visualized trajectories (the lower the cost the more red the trajectory)
This reverts commit 52a4315.
This reverts commit 944a547.
-> reduces oscillations a lot -> no accelerations violations
…Constraints" This reverts commit ad50f92.
This reverts commit a3f4b4b.
adivardi
commented
Oct 23, 2025
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
self-review 1
b47a7a9 to
3e42f4b
Compare
adivardi
commented
Oct 23, 2025
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
self review 2
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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.
Additions & Changes
This PR fixes the nervous steering on the vehicle, and allows good path tracking even in tight serpentines (turning radius 1.4, min vehicle radius 1.35) and when started deviated from path, and with the vehicle delay.
Main changes:
ctrl_sequence_) to initialize the state's velocities for the next iteration, instead of using the odometry feedback. With the slow vehicle motion, using feedback results in next iteration being "stuck" - it is still limited by the feedback which has not/barely changed. by using the last command, we allow the controller to send stronger commands (somewhat break the acceleration limits w.r.t to feedback), but keep the kinematic limits internally (w.r.t to the last command). This is incredibly useful for solving the delay issue.applyControlSequenceConstraints: Previously, u0 was only constrained by the vel min/max. Instead, also constrain it by the accelerations limits w.r.t. the previous control command (stored ininitial_velocities_)Other changes:
4. move the
savitskyGolayFilterto beforeapplyControlSequenceConstraintsas otherwise it may change the control and violate the constraints.5. publish u0 instead of u1
6. color trajectories by cost (see also upstream PR)
7. fix division by zero (see also upstream PR)
Note: For changes 3, 4, 5, I have not opened upstream PRs, as they are partially done by this PR, which reintroduces the ros-navigation#5266 so it is problematic.
See https://app.shortcut.com/enway/story/17718/mppi-vehicle-tuning-stability for documentation about many of the variations I tested and their results.
How to test & expected outcomes
Test with main repo PR
Note: On current
enway-devel, we use PR ros-navigation#5266, so the are not much oscillation, but there is nervous steering. Revert that to see the oscillation (you can cherry-pick 16acda8 onenway-develRecommended reviewer
Georg
[sc-17718]