[mppi] Improve testability to explore parameters for Ackermann steered Polaris

[aosmw]

Feature request

Feature description

Improve testability of nav2_mppi_controller to allow exploration of parameters under unit test like conditions. In other words - help build intuition for the affect of parameter changes by sweeping over parameters of interest and calculating the result of the generated commands without the heavy weight of a full simulation system.

This requires the ability to fully reset internal state, control the random noise seed, tweak critic parameters, apply a simulation function to roughly simulate control system lag/inertia.

I am particularly seeking to gain intuition on what parameters/critics costs/costmap settings are required to get the nav2_mppi_controller to output cmd_vel that reliably approach a target speed of 3.5m/s for an Ackermann steered vehicle (turning radius 2.75m).

[aosmw]

Maybe a "SpeedCritic" will be best way to explicitly desire trajectories/cmds that lead to a target speed... IOW vx_max?

[SteveMacenski]

I'm not entirely sure how to read this ticket's request. If you'd like to make some kind of system for auto-tuning or testing, I'm all for it, but that is going to be something that probably makes sense as a separate repository if its going to be extremely involved.

Maybe a "SpeedCritic" will be best way to explicitly desire trajectories/cmds that lead to a target speed... IOW vx_max?

Sure, its plugin based for a reason, you can add any plugins your like that optimize for what's important to you. I did remark on that a bit in your other ticket #4219 The defaults I set to run at about 0.5m/s so you'd need to retune for higher velocity regimes. I'm sure its possible, you just need to make sure you have a large enough costmap to be able to handle that (i.e. if you have a prediction horizon of 5 seconds at 3.5 m/s, that's a costmap size of 35 meters) and thus a computer which can handle computing long trajectories like this in real-time. With speed comes increased computational burden.

I suppose you could try to either have a very low prediction horizon to stay inside a smaller costmap or keep it longer but have a good chunk of the trajectory off of the costmap, but those are both not optimal setups. Perhaps we need to adjust a couple of the critics to enable that, since right now we prune the path off of the costmap for alignment longer-term and cost critic values off of the costmap are set to max. We could have settings which lets you preform more actions outside of the costmap horizon (and don't negatively score when off of it) but then you run the risk of having collision issues if you only know about an obstacle once its too late to react to it. You'd need a strong low-level safety system to catch such situations and want to run the controller pretty fast (50+hz) to have the best chance of getting around those situations before they become a safety issue once the obstacle gets inside of the costmap window. The costmap size should at the bare minimum be the length of deceleration from full speed to a total stop (2x for both direction) - but you'd want it to be larger to be able to catch obstacle information earlier to plan around it before its a safety issue.

Another direction is if you had vectorized detections rather than relying solely on costmap information, we could include that in a new MPPI critic so that it would take into account collision information outside of the costmap window (i.e. if you have AI detectors, Radar, etc). That would be a general purpose and valuable improvement to MPPI that wouldn't be particularly hard to implement.

Another yet direction is improving the runtime performance so it can run over the larger time horizons, but that is probably going to require a GPU and a bit of effort on your part to GPU-ize MPPI.

[SteveMacenski]

Hey - any updates / thoughts here?

[SteveMacenski]

Closing without update