MPPI has a hard time avoiding an obstacle when it's close

[tonynajjar]

The problem I'm facing is that when the footprint is close to lethal space and trying to go past it, MPPI is very "indecisive", going back and forth.

What you see in the picture is:

• a foorptint close to a lethal obstacle and trying to go past it
• the candidate trajectories visualized by cost (thanks to this PR). More specifically, only the costs of the PathFollowCritic are visualized here
• The pose representing PathFollowCritic's furthest_reached_path_point, used for cost calculation.

I have a strong suspicion that the reason behind the indecisiveness is the battle between the CostCritic and PathFollowCritic. In my config, they are weighted similarly.

There is probably a way to solve this issue by tuning, either with critic weights or offset_from_furthest but imo it would require very specific tuning which will be detrimental to other desired navigational behaviors e.g. path following.

One solution I thought about that could solve this particular without affecting any other behaviors is to only consider trajectories that are not in-collision for the calculation of furthest_reached_path_point. This does however add the requirement for the CostCritic to come before other critics using furthest_reached_path_point.

Curious what you think or if you have any other solution proposals. @mini-1235 maybe you are also interested

[tonynajjar]

One solution I thought about that could solve this particular without affecting any other behaviors is to only consider trajectories that are not in-collision for the calculation of furthest_reached_path_point. This does however add the requirement for the CostCritic to come before other critics using furthest_reached_path_point.

I gave this a quick shot locally and this is what it looks like when the in-collision (magenta) trajectories are not taken into account for the calculation of furthest_reached_path_point