Skip to content

Added support for MPPI Controller to adjust wz_std parameter based on linear speed #5294

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
wants to merge 7 commits into
base: main
Choose a base branch
from

Conversation

vahapt
Copy link

@vahapt vahapt commented Jun 22, 2025

Basic Info

Info Please fill out this column
Ticket(s) this addresses #5274
Primary OS tested on (Ubuntu 24.04 Arm64, Ubuntu 24.04 x64 )
Robotic platform tested on (Gazebo Simulation, Physical Robot)
Does this PR contain AI generated software? (No)

Description of contribution in a few bullet points

Added feature enables dynamic modification of wz_std (angular deviation) based on linear velocity of the robot.

When a robot with high inertia (e.g. 500kg) is moving fast and if wz_std is above 0.3, oscillation behavior can be observed.
Lowering wz_std stabilizes the robot but then the maneuvers take more time.

Dynamically reducing wz_std as vx, vy increase (speed of the robot) solves both problems.
Suggested values to start with: wz_std = 0.3, wz_std_decay_to = 0.05, wz_std_decay_strength = 5.0

The following is used as the decay function
f(x) = (wz_std - wz_std_decay_to) * e^(-wz_std_decay_strength * v_linear) + wz_std_decay_to

Description of documentation updates required from your changes

2 new parameters are added to nav2_mppi_controller, README.md updated accordingly

advanced.wz_std_decay_strength
advanced.wz_std_decay_to

Description of how this change was tested

On both physical and simulated robots, changes deployed and parameters changed through live parameter update.
Robot behavior change and oscillation effects observed and confirmed.

It should be noted that tests are performed with a differential drive robot setup, thus holonomic behavior or potential anomalies with ackermann control are not tested yet.

Unit tests created to test new computeAdaptiveStds function


Future work that may be required in bullet points

  • Decay function might be enhanced based on the user feedback
  • vx_std, vy_std parameters can also be converted to adaptive easily

For Maintainers:

  • Check that any new parameters added are updated in docs.nav2.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
  • Should this be backported to current distributions? If so, tag with backport-*.

Copy link

codecov bot commented Jun 22, 2025

Codecov Report

All modified and coverable lines are covered by tests ✅

Files with missing lines Coverage Δ
...nav2_mppi_controller/models/optimizer_settings.hpp 100.00% <ø> (ø)
...ude/nav2_mppi_controller/tools/noise_generator.hpp 100.00% <ø> (ø)
nav2_mppi_controller/src/noise_generator.cpp 100.00% <100.00%> (ø)
nav2_mppi_controller/src/optimizer.cpp 98.19% <100.00%> (+0.01%) ⬆️

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

… linear speed

Added test cases

Signed-off-by: Vahap Oguz TOKMAK <[email protected]>
@vahapt vahapt marked this pull request as ready for review June 22, 2025 19:15
vahapt added 3 commits June 29, 2025 14:47
… linear speed

- Refactored the code to simplify flow
- Merged to latest `main` branch

Signed-off-by: Vahap Oguz TOKMAK <[email protected]>
… linear speed

- Code cleanup & bugfix

Signed-off-by: Vahap Oguz TOKMAK <[email protected]>
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