Extending capabilities of MPPIController.Optimizer to adjust wz_std parameter based on linear speed

[vahapt]

Feature request

Feature description

Currently, Optimizer optimizer_ is declared as a value-based object in the MPPIController class. This approach does not allow subclasses of MPPIController to override or customize the behavior of Optimizer. Declaring optimizer_ as a pointer (e.g. std::unique_ptr<Optimizer>) would allow us to replace it.

Implementation considerations

There will be an insignificant slowdown due to added pointer lookup (probably a few nanoseconds). But added polymorphic behavior is worth it.

I can create a PR if there are no objections.

[SteveMacenski]

Please test the performance changes but overall no objection if there’s no meaningful impact.

If you’re changing the Controller to use a specialization of the Optimizer though, I don’t see how the number of line changes one way or another is very different. You can construct the specialization instead of the Optimizer in the cpp file vs changing the Controller’s hpp optimizer type.

I’d be curious to hear about your application / customization if you could share!

[vahapt]

You can construct the specialization instead of the Optimizer in the cpp file vs changing the Controller’s hpp optimizer type.
I'm curious to understand how this is done. My way of thinking is polymorphism, introducing virtual functions and override them with custom behavior. As I understand, you've a better way I'm eager to hear.

What I'm trying to do is to add this behavior:

void CustomOptimizer::updateControlSequence()
{
    auto & s = settings_;
    auto current_vx = fabs(state_.speed.linear.x);
    float e = std::exp(1.0f);
    float decayed_wz_std = (wz_std_start-wz_std_decay_to) * powf(e, -1 * wz_std_decay_strength * current_vx) + wz_std_decay_to;
    s.sampling_std.wz = decayed_wz_std; // replace the value of wz_std internally with our decayed version
    //RCLCPP_INFO_STREAM(logger_, "current_vx: " << current_vx << " wz_std: " << wz_std_start << " decayed_wz_std: " << decayed_wz_std);

    mppi::Optimizer::updateControlSequence();
}

wz_std (angular deviation) gets smaller as vx (speed of the robot) increases which eventually minimizes oscillation behavior when robot is moving fast and it has high inertia (e.g. 500kg)

Sample output of the decay function

[SteveMacenski]

Got it - any interest in just contributing this to MPPI directly? Seems like a nice feature that could be parameterized on/off for users desires. I think this is sensible if that works for you!

The hyperlink doesn't appear to be live or point somewhere private that I cannot see.

[vahapt]

any interest in just contributing this to MPPI directly?

There are 3 points I'm considering:

• There's a similar issue [MPPI] Question on Motion model Lag Compensation #5214 which probably will solve the problem in a better way.
• Adding more variables to a complicated controller increases the learning curve for newcomers, we should keep things as simple as possible.
• I'd love to battle test and improve the algorithm before committing.

Having said that, I'd be more than happy to commit my changes to MPPI directly if you insist. The following parameters should be declared which allows us to configure curvature of the graph:

• wz_std_decay_to
• wz_std_decay_strength

[SteveMacenski]

I would love to have the contribution. I think it makes sense to add them -- maybe under and advanced namespace parameter (i.e. <controller name>.advanced.wz_std_decay_to whereas the defaults are essentially disabled (and state that explicitly in the configuration guide alongside the use description). Reducing the learning curve is important, but missing important capabilities takes priority for a production stack like Nav2. At least this way we can mark them as advanced so folks don't mess with them until they're really ready. MPPI has alot of highly technical parameters already so I don't think this is a bridge too far at all.

I'd be happy to field a PR whenever you feel its ready! With respect to the ticket, its still in the air if the taus are going to be added. Recent discussions have pointed to other changes as having had a larger impact so its unclear if that is really going to be needed. So, I wouldn't make decisions based on that for the time being.

[vahapt]

Agreed. That solves my main issue and one less package to manage on my side :)
I'll create a separate ticket and create my PR regarding the decay function.

Regarding this ticket, we should also introduce some virtual functions to create an entry point for inherited Optimizer class. e.g. virtual geometry_msgs::msg::TwistStamped evalControl(....)

I'm generally leaned towards extensibility, flexibility but I also need to understand the team's conventions first. I'll share a separate PR on this and let you/team decide it.

[SteveMacenski]

I think you can just use this ticket to keep everything in one place & just rename it!

[SteveMacenski]

W.r.t. the extensibility: these are themselves plugins and usually places that are intentional for extension we make into plugins (so the controller is a plugin, the objective critic functions are a plugin, etc). I don't think of the optimizer being a point of custom configuration for an application, that should be general purpose for all applications. If there are improvements to be made in Optimizer, it usually be best to be contributed upstream so that it can be benefited from all users since I don't think there are any proprietary / highly application specific mods to be made in the Optimizer (unlike the critics whereas specific application constraints may need to be modeled - or the plugins themselves which can be swapped out for the desired robot, environment, or behavior).

I suppose I'm not opposed to making the Optimizer more customizable, but I'd rather do that in response to an issue rather than proactively since I'd like to encourage contributions of that back into the stack. As you showed, those changes are general purposes and can be contributed back to the stack and aren't 'secret sauce' or highly application dependent specializations like the Critics

If we added some inheritance items, that would still involve forking and modifying the software since there is no plugin factory for Optimizer specializations.

[vahapt]

Point very well taken.

[vahapt]

@SteveMacenski This is getting deeper :)

NoiseGenerator is copying settings_ only when initialize() and reset() functions are called. As I understand, this is done intentionally for thread safety. However it creates the following bug:

When we do a live change vx_std, vy_std and wz_std parameters, it is not reflected until the navigation modules are restarted. Tough, it's applied to scoring section live.

void NoiseGenerator::initialize(
...
ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx);
ndistribution_vy_ = std::normal_distribution(0.0f, settings_.sampling_std.vy);
ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz);

From user perspective (I had witnessed it) this creates inconsistent and unexplained behavior.

Reflecting wz_std on every step requires passing some form of referenced variable and updating it in a thread safe way (e.g. when generateNextNoises() is called, re-create ndistribution_wz_)

My humble suggestion is to fix the bug while adding the feature. What do you think?

[SteveMacenski]

We could pass it in to generateNextNoises or setNoisedControls (depends if we want to have it applied in this iteration or the next iteration). Admittedly generateNextNoises feels ergonomically the right place, but that would impact the next iteration, not the current one.

Then, we can store the "adaptive std's" to use. initialize and reset would just need to be updated to set that "adaptive std's" back to the parameterized std.

The threading is OK as long as the updates don't happen after generateNextNoises is called (in the call or before the call). That function triggers a conditional variable that wakes a thread to generate the new noises that could take an arbitrary amount of time when regenerate_noises_ = true.

What we could do is give generateNextNoises the 3 STDs to use. If any are non-zero (or nullptr or negative or whatever) even if regenerate_noises_ = false, we store and only generate that axis. So we could then pass it in the wz std and regardless of regenerate_noises_ = false we generate only that one axis that is strictly required. If regenerate_noises_ = true, then we automatically regenerate them all anyway and we store to use the one given if non-zero (or nullptr or negative or whatever).

---

When a dynamic parameter is changed via RQT, it triggers the post params callback

navigation2/nav2_mppi_controller/src/optimizer.cpp

Line 107 in 829e683

parameters_handler_->addPostCallback([this]() {reset();});

which triggers a reset. Part of that reset should be setting settings_ to the noise generator.

[vahapt]

I'm getting a bit lost here, setNoisedControls is just collecting what has been generated on previous processing loop? So change to "adaptive std's" can only be applied on the next iteration anyway.

If we agree on the above, generateNextNoises feels correct place to process "adaptive std's" to me.

would just need to be updated to set that "adaptive std's" back to the parameterized std.

We always need the original value of parameterized std to calculate adaptive one. So SamplingStd.wz will always be parameterized std. adaptive std will be another variable.

I forgot to mention that I've switched to main branch to avoid massive merge conflicts. I'll cancel current PR and commit with a new (and proper) PR from main.
Bug I'm referring may not exist on jazzy branch. On main branch, ndistribution_vx_, ... are only initialized and not changed even if reset() is called.

When a dynamic parameter is changed via RQT, it triggers the post params callback

That's enlightening, filled the gap. Thank you.