Add acceleration constraints in the MPPI controller

[Ayush1285]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3351
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation of tb3
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Updating MPPI controller to consider acceleration and deceleration constraints.

Description of documentation updates required from your changes

• Added new parameter, so need to add that to default configs and documentation page

Future work that may be required in bullet points

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

[mergify[bot]]

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues. You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify[bot]]

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues. You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[osama-z-salah]

@Ayush1285 I have tried your PR, but it seems like there is an issue with achieving the maximum linear velocity (vx_max). Can you confirm if you have tested this with the robot, and did you observe any similar behavior?

[Ayush1285]

@Ayush1285 I have tried your PR, but it seems like there is an issue with achieving the maximum linear velocity (vx_max). Can you confirm if you have tested this with the robot, and did you observe any similar behavior?

I haven't tested the behaviour of maximum linear velocity. Ideally we shouldn't be having this problem. Can you please tell me what were the parameters you used? So that I can also test using same parameters.

[Ayush1285]

@Ayush1285 I have tried your PR, but it seems like there is an issue with achieving the maximum linear velocity (vx_max). Can you confirm if you have tested this with the robot, and did you observe any similar behavior?

@osama-z-salah I tried with following parameters: vx_max = 0.5 m/s, ax_max = 2.0 m/s^2 Plot of vx with acceleration constraints

Plot of vx without acceleration constraints

It goes near to the max linear velocity with acceleration constraints also.

[SteveMacenski]

It goes near to the max linear velocity with acceleration constraints also.

But it doesn't reach it. I've seen this before when I once tried to limit the velocities in the motion model to its maximum. I think what I determined at the time was that we clipped samples at the maximum speed so when we score + find the next optimal, it'll never achieve the maximum speed due to the soft-max averaging. This is why the constraint critic was introduced instead so we punish velocities out of bounds but don't bound the velocities being scored themselves.

I think may require us to add the acceleration constraints to the ConstraintCritic - perhaps instead of doing it in the motion model. Or maybe spend some time thinking about why the acceleration is being limited artificially. Since this is on acceleration, I wouldn't have expected this issue to occur and this is where the MPPI technique applies the dynamics models to roll out control velocities into actual velocity candidates.

With that said, I also pointed out an actual computational bug in my last review, so its possible this issue just goes away after that's fixed. I'll also take a look over the code again with this in mind to see if I don't see something else. That bug though would cause some issues so it wouldn't surprise me if that's just it.

---

Linting:

Diff with 5 lines
--- include/nav2_mppi_controller/motion_models.hpp
+++ include/nav2_mppi_controller/motion_models.hpp.uncrustify
@@ -130 +130 @@
-   0.0f};
+    0.0f};

[Ayush1285]

It goes near to the max linear velocity with acceleration constraints also.

But it doesn't reach it. I've seen this before when I once tried to limit the velocities in the motion model to its maximum. I think what I determined at the time was that we clipped samples at the maximum speed so when we score + find the next optimal, it'll never achieve the maximum speed due to the soft-max averaging. This is why the constraint critic was introduced instead so we punish velocities out of bounds but don't bound the velocities being scored themselves.

I think may require us to add the acceleration constraints to the ConstraintCritic - perhaps instead of doing it in the motion model. Or maybe spend some time thinking about why the acceleration is being limited artificially. Since this is on acceleration, I wouldn't have expected this issue to occur and this is where the MPPI technique applies the dynamics models to roll out control velocities into actual velocity candidates.

With that said, I also pointed out an actual computational bug in my last review, so its possible this issue just goes away after that's fixed. I'll also take a look over the code again with this in mind to see if I don't see something else. That bug though would cause some issues so it wouldn't surprise me if that's just it.

Linting:

Diff with 5 lines
--- include/nav2_mppi_controller/motion_models.hpp
+++ include/nav2_mppi_controller/motion_models.hpp.uncrustify
@@ -130 +130 @@
-   0.0f};
+    0.0f};

The problem was due to the above bugs only, thanks for catching them :) Now it goes up to maximum velocity.

This is the plot of acceleration, it remains within specified constraints(2.0 m/s^2) for all iterations. Except 1, where it spikes to 2.5

[SteveMacenski]

@Ayush1285 to fix that, we just add to the applyControlConstraints which I think you haven't done yet. That should square all this up for merging with the other comments above :-)

[SteveMacenski]

I believe documentation on docs.nav2.org is the only thing left? The configuration + migration guide

Edit: oh, and the applying control constraints after the soft max to have the hard constraints applied!

[Ayush1285]

PR for the docs update: https://github.com/ros-navigation/docs.nav2.org/pull/562

[codecov[bot]]

Codecov Report

All modified and coverable lines are covered by tests :white_check_mark:

Files	Coverage Δ
...nav2_mppi_controller/models/optimizer_settings.hpp	100.00% <ø> (ø)
...ler/include/nav2_mppi_controller/motion_models.hpp	100.00% <100.00%> (ø)
nav2_mppi_controller/src/optimizer.cpp	100.00% <100.00%> (ø)

... and 8 files with indirect coverage changes

[SteveMacenski]

Nitpick and I can merge (you have your branch setup so I can't push it)

[RBT22]

What do you think about the practical usability limits of this approach? Similarly to my previous attempts implementing acceleration limits to MPPI, I experienced unpredictable behaviors. Lowering the acceleration limits causes the robot (TB3, Iron) to sway, stutter, or completely disregard it's path. This behavior can be observed from around <1.0 m/s^2.

Setting accelerations to 0.5 m/s^2 resulted in the following for me:

[Ayush1285]

What do you think about the practical usability limits of this approach? Similarly to my previous attempts implementing acceleration limits to MPPI, I experienced unpredictable behaviors. Lowering the acceleration limits causes the robot (TB3, Iron) to sway, stutter, or completely disregard it's path. This behavior can be observed from around <1.0 m/s^2.

Above 2 m/s^2 should be fine I guess, I've also faced similar issue with lower acceleration bounds.

[Ayush1285]

@SteveMacenski what do you think about this stalling behaviour of bot at low acceleration bounds? It occurs even at 1.5 m/s^2. Screencast from 05-31-2024 11:10:57 PM.webm

In this scenario, even though the axes are aligned to the path, it is still not moving forward.

[SteveMacenski]

What do those acceleration graphs look like in RQT?

Lowering the acceleration limits causes the robot (TB3, Iron) to sway, stutter, or completely disregard it's path. This behavior can be observed from around <1.0 m/s^2.

Wrt your videos, did you try retuning? It seems like the robot just doesn't want to leave the zero-costed space, which makes sense to me with low constraints. If you remove the obstacle critic (just for experimentation), does it go back to normal? It may be that we just need to tweak the cost/obstacle critic defaults.

---

I'm noting the the TB3 params in DWB (also used by linobot) were set as 2.5 m/s2 and 3.2 rad/s2 which are above the threshold for concern? I'm doing a quick survey and see joystick teleop for fetch is 1.0m/s2, clearpath husky is 3.0m/s2.

That's a sampling of robots big, small; indoor and outdoor. I think in truly low acceleration applications that this is perhaps not the silver bullet solution, but I don't think many robots require sub-1m/s2 acceleration constraints so as long as this works for the "normal" range of values, I think we're fine (?). For really weird, specialized situations, there are going to be more optimal algorithms to help you, so I think super-low acceleration counts as part of that "weird, specialized" class of robot systems which not many fall into.

Imagine you had a robot with 0.1 m/s2 acceleration constraints. If the path tracker handled that perfectly, that's such a slow acceleration profile I think you would strictly need to have a planner that takes into account the dynamic constraints into its formulation because you couldn't just make a turn, you'd have to slow down waaaay before that turn to turn super duper slowly, then ramp back up -- assuming it moves at a fast clip at full speed. So between 0.1 and 1 m/s2 (where we handle well I'm told above), where do we draw that line where its no longer an effective general acceleration profile for path tracking requiring some specialized development to make plausible?

But regardless, I think the issue your videos show is related to tuning it seems? Something like 0.5m/s2 should probably work (especially at low maximum speeds). I wouldn't try to go in a rabbit hole to make anything lower work since that's hitting the point that even if the robot moves, its so constrained it may not be possible to turn at any reasonable velocity. Most robots that I know that have very low acceleration profiles also move slower than snails to compensate (i.e. you can't have a runaway robot that takes 10m+ to stop).

[Ayush1285]

Wrt your videos, did you try retuning? It seems like the robot just doesn't want to leave the zero-costed space, which makes sense to me with low constraints. If you remove the obstacle critic (just for experimentation), does it go back to normal? It may be that we just need to tweak the cost/obstacle critic defaults.

yes, it works fine if I remove the Cost Critic. It is a kind of trade-off then, if we reduce the value of Acceleration constraints then we will have to adjust Obstacle and cost critic also.

[Ayush1285]

But regardless, I think the issue your videos show is related to tuning it seems? Something like 0.5m/s2 should probably work (especially at low maximum speeds).

yeah, after removing the Cost critic, it works even at 0.5 m/s^2.

[SteveMacenski]

yeah, after removing the Cost critic, it works even at 0.5 m/s^2. yes, it works fine if I remove the Cost Critic.

Seems then just like a tuning thing, I'm not worried about it then!

I've asked another maintainer to review and give his feedback since this is something they will plan to use as well. Besides that, anything else blocking merging?

[Ayush1285]

yeah, after removing the Cost critic, it works even at 0.5 m/s^2. yes, it works fine if I remove the Cost Critic.

Seems then just like a tuning thing, I'm not worried about it then!

I've asked another maintainer to review and give his feedback since this is something they will plan to use as well. Besides that, anything else blocking merging?

I've just updated the test for the positive value of ax_min constraints. I don't think anything else is remaining. I just wanted to run system tests to be sure, but if it is not possible now then we can go ahead.

[doisyg]

Great to see work is being conducted on this, thank you all. I had a first look. First a bit of context. My robotic platform is a massive 600 kg one with specific kinematic constraints: 0.25m/s2 for acceleration and 0.5m/s2 for deceleration on the linear axis. And both 1.2rad/s2 of accel/deccel on rotation (Z). And it is fully bidirectional.

I did not properly dig on the implementation, but from the parameters available:

• ax_min / ax_max
• ay_max
• az_max

it looks like this PR intent is to deal only with acceleration, not deceleration (or I will assume to see more parameters for ay and az). If the intend here is to have different linear accelerations limits when moving forward and backward, then you should check that ax_min doesn't become the deceleration limit when moving forward, and symmetrically that ax_max doesn't become the deceleration limit when moving backward. (I believe that's the case here).

However, I would be much in favor in dealing with decceleration properly, in a similar way as it is done for the velocity smoother, but be careful, it is easy to mix up when also dealing with bi-directionality. See for https://github.com/ros-navigation/navigation2/pull/3529 instance.

Finally I gave it a spin without setting the new parameters, to check that it was not breaking existing tuning. It looks okay. I did not test yet the behavior when setting the parameters as it is unclear who should be set / expected for my bi-directional platform when different accel and deccel constraints.

[Ayush1285]

it looks like this PR intent is to deal only with acceleration, not deceleration (or I will assume to see more parameters for ay and az).

@doisyg It will deal with both acceleration and deceleration. We've assumed that the value of acceleration and deceleration constraints would be same for the y-axis(in the case of an omnidirectional robot) and the z-axis(rotational axis) in most cases. Hence there is only one parameter for y and z axes.

[Ayush1285]

If the intend here is to have different linear accelerations limits when moving forward and backward, then you should check that ax_min doesn't become the deceleration limit when moving forward, and symmetrically that ax_max doesn't become the deceleration limit when moving backward.

I think the naming of the parameters may be confusing. They are not the limits of acceleration while moving forward and backward. ax_max is maximum acceleration along the x-axis, and ax_min is maximum deceleration(braking power).

[SteveMacenski]

@doisyg I think @Ayush1285 did a good job explaining. Would you want different constraints for reversing acceleration/deceleration than forward direction? I can see the sense in setting different velocity bounds for robots with a directional preference, but it didn't occur to me someone might want higher order things to also have different limits for forward vs reverse.

[doisyg]

Thank you both for explaining! I don't think we need different constraints for forward and backward X movement (at least I don't). But definitely different accel and deccel constraints. So all good. I don't need different accel/deccel for rotation, but I could see someone needing it. We don't necessarily need to address that now, but just noting.

Another point to note, this PR will not help smoothing the transition between speeds generated by MPPI and the 0.0 twist sent by the controller server once the goal checker kicks in. But that's totally normal (for me this is handled by the velocity smoother). I will make an experiment later to check if this work can actually help achieving better goal accuracy by taking into account the decel constraint of the robot (and with smaller goal checker thresholds), which would be amazing.

If you are all happy, despite my limited testing, this is good to go. I mainly tested that it doesn't break the current default behavior. About the general impact of adding appropriately tuned kinematic constraints, that will need more continuous testing on my end, but I can assess after merging and a couple of days/weeks of live run.

[SteveMacenski]

Sounds good!

[SteveMacenski]

Thanks @Ayush1285 for this awesome work, it is much appreciated and will shortly be in use by a number of companies worldwide! This has been a big ask for MPPI! Any interest in other MPPI improvement projects (https://github.com/ros-navigation/navigation2/issues/3351) or other things in Nav2 I can help you through?

[Ayush1285]

Thanks @Ayush1285 for this awesome work, it is much appreciated and will shortly be in use by a number of companies worldwide! This has been a big ask for MPPI! Any interest in other MPPI improvement projects (#3351) or other things in Nav2 I can help you through?

Thanks Steve for your insights on performance improvement and other reviews. Further, I am interested in trying out Eigen for MPPI and potential optimization of collision checker object. I can work on these two items.

[SteveMacenski]

Fantastic! There's been a chat on Eigen in https://github.com/ros-navigation/navigation2/issues/4237 if you hadn't already seen that is good to reference. I think that's the most bang-for-your-buck thing to do so I'd recommend doing that first (the collision checker object also important, but I think Eigen could be a 20-30% improvement, whereas collision checker 10-20%. Still high, but this is higher)

A user said they had good experiences once actually using the Eigen tensor library and the blog post (https://romanpoya.medium.com/a-look-at-the-performance-of-expression-templates-in-c-eigen-vs-blaze-vs-fastor-vs-armadillo-vs-2474ed38d982) makes me feel very optimistic it can help. I've profiled MPPI in detail many times and a non-trivial amount of time is spent now fully internal to xtensor views and strides. If Eigen can do that even 5% faster, that's ~2-3% improvements in the performance. And those metrics claim it could be up to 8x faster. So we could potentially double performance overnight conceptually (though I'd settle for much less :laughing: )

[Ayush1285]

Fantastic! There's been a chat on Eigen in #4237 if you hadn't already seen that is good to reference. I think that's the most bang-for-your-buck thing to do so I'd recommend doing that first (the collision checker object also important, but I think Eigen could be a 20-30% improvement, whereas collision checker 10-20%. Still high, but this is higher)

A user said they had good experiences once actually using the Eigen tensor library and the blog post (https://romanpoya.medium.com/a-look-at-the-performance-of-expression-templates-in-c-eigen-vs-blaze-vs-fastor-vs-armadillo-vs-2474ed38d982) makes me feel very optimistic it can help. I've profiled MPPI in detail many times and a non-trivial amount of time is spent now fully internal to xtensor views and strides. If Eigen can do that even 5% faster, that's ~2-3% improvements in the performance. And those metrics claim it could be up to 8x faster. So we could potentially double performance overnight conceptually (though I'd settle for much less 😆 )

Thanks!! I'll check the above links and start on them in my leisure time.

though I'd settle for much less 😆

That's good to know :laughing:

[Ayush1285]

@SteveMacenski Where can we continue the discussion on Eigen for MPPI? I've tried the first draft(in my fork repo), which is not optimised currently.

[SteveMacenski]

@Ayush1285 wow, that was quick and sounds awesome! I think this thread is the right spot! https://github.com/ros-navigation/navigation2/issues/4237