Skip to content

Conversation

@adivardi
Copy link
Collaborator

@adivardi adivardi commented Oct 23, 2025

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:

  1. Revert PR Unclamp noise velocity. ros-navigation/navigation2#5266: This PR allows good path tracking and reduces the oscillations due to low acceleration limits. However, it causes the optimal control to jump too much from side to side to compensate for the low accel limits & delay, resulting in the "nervous steering" (i.e. small oscillations of the wheels steering)
  2. Use the previous optimal control (u1 from previous 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.
  3. in 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 in initial_velocities_)

Other changes:
4. move the savitskyGolayFilter to before applyControlSequenceConstraints as 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

Note to the reviewer: The instructions below might not be complete. If you think more tests are necessary, please do not hesitate to perform them and to record the result in a comment in this PR.

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 on enway-devel

Recommended reviewer

Georg

[sc-17718]

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
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)
-> reduces oscillations a lot
-> no accelerations violations
@adivardi adivardi marked this pull request as draft October 23, 2025 10:49
Copy link
Collaborator Author

@adivardi adivardi left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

self-review 1

@adivardi adivardi force-pushed the av/mppi_feedback_clean branch from b47a7a9 to 3e42f4b Compare October 23, 2025 11:09
Copy link
Collaborator Author

@adivardi adivardi left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

self review 2

@adivardi adivardi changed the title Av/mppi feedback clean Use last command to initialize state - fix oscillations & nervous steering Oct 23, 2025
@adivardi adivardi marked this pull request as ready for review October 23, 2025 11:31
@adivardi adivardi requested a review from georgflick October 23, 2025 11:31
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants