Added support for MPPI Controller to adjust wz_std parameter based on linear speed #5281
Conversation
… linear speed Signed-off-by: vahapt <5289529+vahapt@users.noreply.github.com>
There was a problem hiding this comment.
I appreciate the good docs in the readme. Please add that in the config guide too https://docs.nav2.org/configuration/packages/configuring-mppic.html
nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp
Outdated
Show resolved
Hide resolved
nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp
Outdated
Show resolved
Hide resolved
nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp
Show resolved
Hide resolved
| } | ||
|
|
||
| //boundary check, if wz_std_decay_to is out of bounds, print a warning | ||
| if (s.sampling_std.wz_std_decay_to < 0.0f || s.sampling_std.wz_std_decay_to > s.sampling_std.wz) |
There was a problem hiding this comment.
Can we check this in getParams to disable it and warn once on startup? The continuous warning here would be annoying.
There was a problem hiding this comment.
If the value is changed after startup we should warn again. I'll adjust it to print only once per change.
| @@ -419,8 +460,11 @@ void Optimizer::updateControlSequence() | |||
| xt::noalias(costs_) += | |||
| s.gamma / powf(s.sampling_std.vx, 2) * xt::sum( | |||
| xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); | |||
|
|
|||
| float wz_std = calculateDecayForAngularDeviation(); | |||
There was a problem hiding this comment.
Wouldn't we want to reduce the next iteration's wz_std appropriately? This just impacts the scoring, not reducing the std used in the future for sampling to create less options. I'd like to know more about how you came up with this modification and if it has theoretical backing. I thought this was going to reduce wz_std used in sampling with speed, not only the computation of the optimal trajectory from a fully s.sampling_std.wz-sampled batch
There was a problem hiding this comment.
Wouldn't we want to reduce the next iteration's wz_std appropriately?
That's correct, I probably missed the actual point and just noticed same value is also used in NoiseGenerator.
Just to make sure we're on the same page, we should also be applying calculated value to this section (noises_wz_) of the code right?
void NoiseGenerator::generateNoisedControls()
{
auto & s = settings_;
xt::noalias(noises_vx_) = xt::random::randn<float>(
{s.batch_size, s.time_steps}, 0.0f,
s.sampling_std.vx);
xt::noalias(noises_wz_) = xt::random::randn<float>(
{s.batch_size, s.time_steps}, 0.0f,
s.sampling_std.wz);
if (is_holonomic_) {
xt::noalias(noises_vy_) = xt::random::randn<float>(
{s.batch_size, s.time_steps}, 0.0f,
s.sampling_std.vy);
}
}
I'd like to know more about how you came up with this modification and if it has theoretical backing.
Initial idea for the modification came up with observing the oscillation behavior of the physical robot.
While the robot is moving I've changed wz_std parameter through rqt. Thus, I should have actually executed future for sampling to create less options properly.
With today's tests I've observed similar stability behavior, apparently it was not the best.
if it has theoretical backing
It's mostly based on field observations. To add some theory, while we're driving a car, the forces exerted on front wheels tends to move the wheels to the center. This implicitly changes driver behavior while steering. I wanted to implement something similar.
Please correct me if I'm missing something.
There was a problem hiding this comment.
we should also be applying calculated value to this section (noises_wz_) of the code right?
Yes! Note that regenerate_noises would need to be true with this feature since we'd need to regenerate the noises each iteration since the wz would change (though maybe we could have it only regenerate those that changed values in this case, only wz's.
While the robot is moving I've changed wz_std parameter through rqt.
Got it, so you didn't test the code in this PR yet, correct? This wouldn't have the impact you had in mind, I don't think. I also think that the change of wz then might want to happen in the main function of the optimizer, not in the control sequence -- that way we update the wz to be sampled in a given iteration with the given robot speed. For instance, in the prepare function. Then, the wz should be reset to the parameterized value in the reset function for the next MPPI path to track.
if it has theoretical backing
This was geared towards the question when this was only updating wz used in the cost scoring. If we update it for the actual noised trajectory sampling + scoring, then this is totally fine to change as long as it is consistent within an iteration and the jumps aren't too discontinous.
There was a problem hiding this comment.
Note that regenerate_noises would need to be true
It istrue
Got it, so you didn't test the code in this PR yet, correct?
I did test and strangely observed the behavior we were expecting. Once implemented properly, maybe I will be able to reduce the strength from 5.0 to 1.0~2.0.
I also think that the change of wz then might want to happen in the main function of the optimizer, not in the control sequence -- that way we update the wz to be sampled in a given iteration with the given robot speed. For instance, in the prepare function.
Agreed.
Then, the wz should be reset to the parameterized value in the reset function for the next MPPI path to track.
For that part I'm thinking to introduce another wz_optimized (or wz_clamped) internal variable instead of backing up wz, processing it and then restoring it. That would end up in a cleaner code.
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
…raints.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
…raints.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
|
I see a bunch of things closed and a re-review request, but I don't see any changes pushed |
I misclicked and github does not provide cancel button :) |
|
hah - no worries |
Basic Info
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
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.
Default configuration disables this feature, thus regression risks are minimal.
Future work that may be required in bullet points
For Maintainers: