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

[vahapt]

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-*.

[codecov]

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.

[SteveMacenski]

This'll cause linting issues

[SteveMacenski]

This doesn't resolve the issue that the noise generator's wz sampling doesn't actually change when regenerate_noises = false. We need to regenerate the wz samples on getNextNoises when regenerate_noises = false regardless to reflect on the newest values.

This seems to have undone some points of previous discussion and create other gaps. I've identified a few bugs / logical gaps that need to be addressed.

Overall, I think this is what we need to do:

• Revert all unecessary changes. We shouldn't be messing with the thread, state variables, constructors, etc.
• Add the adaptive wz to the SamplingStd class along with validate, compute and reset (to reset back to original value). These are members of the wz std sampling data and advanced optimizer settings, not the noise generator utility.

From there, the data and its operating functions will all be grouped together and we just need to figure out where to call them.

I'd argue it should be in the optimizer:

• The wz adaptive should be reset and validated on Optimizer::Reset (validate can be called within reset)
• The wz adaptive should be computed and validated on generateNoisedTrajectories (validate can be called within the compute function implementation)
• The new wz adaptive value can be given to generateNextNoises as an argument
• If the current wz is different and regenerate_noises = false, then store the new value and recompute wz. If the current wz is different and regenerate_noises = true, then store and call the thread to make the full update. If current wz is the same as the stored wz, do nothing more than it does now.

I'd then argue that perhaps moving wz_std_adaptive from SamplingStd to AdvancedConstraints makes sense to bin it altogether. And that way we pass in the AdvancedConstraints to the generateNextNoises which could later contain other Vx/Vy noises to regenerate based off of, from speed changes as well in the future. We just need to make sure we keep SamplingStd.wz and AdvancedConstraints.wz_std_adaptive aligned in all the right places.

---

The other way I could see this going is that we do it in the noise generator. As an implementation detail of generateNextNoises we update and validate the adaptive wz using the methods in the AdvancedConstraints.

If the current wz is different and regenerate_noises = false, then store the new value and recompute wz. If the current wz is different and regenerate_noises = true, then store and call the thread to make the full update. If current wz is the same as the stored wz, do nothing more than it does now.

In the noise generator reset, we call the adaptive wz reset method back to the base settings value.

The downside of this is that then the current state of the adaptive wz is managed by a utility rather than the optimizer, which is a little odd to have to query to get the state for the updateControlSequence method. Especially considering that the AdvancedConstraints is a member of the OptimizerSettings so these are optimizer-specific states

[SteveMacenski]

This should not need to be stored here. We talked about this being in the std model alongside the validate / update / reset functions

[vahapt]

I still strongly disagree. It's not a setting. It's a part of current state just like vx.

[SteveMacenski]

But its not part of the noise generator state, its part of the optimizer state

[vahapt]

Comments noted, I'll minimize changes and leave potential improvements to later updates.

Add the adaptive wz to the SamplingStd class along with validate, compute and reset (to reset back to original value). These are members of the wz std sampling data and advanced optimizer settings, not the noise generator utility.

So, we'll convert adaptive_wz back to shared_pointer and move functions to SamplingStd?
It feels like adding controller functionality to a model object. (referring to MVC pattern). Not my style, but I'll do it if you insist.

Note: My work schedule is a bit hectic within this month, please bear with me.

[SteveMacenski]

So, we'll convert adaptive_wz back to shared_pointer and move functions to SamplingStd?

I don't think we need to change it to a shared pointer, we can just pass in the updated / validated adaptive Stds for the noise generator computation from the main Optimizer settings_ variable that is being managed by the primary controller functions. But otherwise, yes!

Something we could do also/instead is change the settings itself to being a shared pointer. We pass it into a few places and it is not intended that these aren't updated in their respective places, I don't think.

[vahapt]

If we don't keep a pointer we'll have 2 copies of settings hence 2 copies of adaptive stds. Reset is called only once per waypoint but we update adaptive std on every tick.

Updated version of adaptive std values are both accessed inside noise generator and optimizer.

I think we can make settings_ a unique pointer owned by the Optimizer. Noise generator's thread can create a copy of required variables at initialization phase to avoid concurrent modifications.

[SteveMacenski]

Updated version of adaptive std values are both accessed inside noise generator and optimizer.

I think that's one of the key points I want to change about the current implementation. I would like the validate and update to happen in the Optimizer so that the flow of validate/update/reset are clear as day and only used in a single object so that it can be more easily understood the control flow. Also we may update these values over time based on other criteria (std vx, vy based on the velocity as well, perhaps) so my thinking of the separation of concerns in this package wouldn't put this in the noise generator. Its goal is just to generate noises, not compute the values to use for noises. That seems to me more an optimizer setting / state which the noise generator is just a user utility of.

We could do something like the constraints vs base_constraints since that is a workflow already employed in this project. The base constraints are the base non-modified stds set by settings, which are used to initialize the std's to start and on reset. The std's otherwise can be modified as a state & we pass in the current std's state to the noise generator for generating new noises. For each of the std axes, if its the same and regenerate noises is false, then we do nothing. If regenerate noises is on, we regenerate. If the axes are different and whether its on or off, we set the new std and regenerate for that given axis. Then, rather than storing settings_ in the noise generator, we just extract the things we want, i.e. sampling_std, batch_size and time_steps. That would also resolve the multiple copies of settings_ which can be less immediately clear to the readers (and not the intention of the developer).

[BriceRenaudeau]

Hi, thanks for this PR.
We also developed a dynamic wz_std. The goal was to get a fixed spread of the trajectories (representing the size of the avoidable obstacles) even when the max_speed changes. But we didn't make it real time.

Another point that may cause oscillations on heavy robots is the "latency" of the mechanical control. We work to integrate this in the motion_model to take it into account.

[SteveMacenski]

Let me follow up with you about that over email @BriceRenaudeau !