Dynamically compute prune_distance

[shrijitsingh99]

Bug report

Required Info:

• Operating System:
  • Ubuntu 18.04
• Version or commit hash:
  • from source: master

Clarifying the discussion from #938

Steps to reproduce issue

Bring up the robot and set paramters as mentioned below.
Also add a small wall using Gazebo as shown in the first figure.

Context

Currently, prune_distance is a set to a static value.
There issues with setting this to a static value:

1. Max velocity cannot be achieved if the value is too small
   Eg: If the max_speed_xy = 1.2, sim_time = 2.0 and prune_distance = 1.0
   Then the max speed attainable is limited to prune_distance / sim_time = 1.0 / 2.0 = 0.5m/s since trajectories with higher velocities than this will cross the last pose of the prune global plan i.e. if velocity is 1.2m/s then the last pose of the trajectory will be 1.2 * sim_time = 1.2 * 2.0 = 2.4m. Since 2.4 > prune_distance such trajectories will be discarded.

2. Local minima during u-turns
   So to counter the first issues, one solution is to set the value of prune_distance to something large for large max velocities. Let us assume the prune_distance is set to 2.5m which will allow achieving max_speed_xy = 1.2 with sim_time = 2.0.
   This leads to the robot getting stuck in a local minima during u-turns.
   For example in the image below:

On scaling the cost values between 0 - 100, we get a better visualization of what is happening:

As you can see from the above example the GoalDistCritic is biasing the robot to not move ahead and go around the obstacles since that will cause the robot to move away from the goal before it takes a u-turn. Such trajectories will have high cost and will be discarded, instead the robot will oscillate in place and get stuck.

From this we can draw the conclusion that the GoalDistCritic is overpowering the PathDistCritic so one solution might be to disable the GoalDistCritic and only rely on PathDistCritic since it should be enough to guide the robot along the global plan. But this causes the robot to get stuck and not move at all as PathDistCritic causes the robot to stay near to the global plan it doesn't penalize to robot for not moving forwars i.e. along the global path. This happens as the cost along the global path doesn't decrease as you move along the path, the cost only increases as you move away from the path. In short there is no decreasing gradient along the global path which will incentivise the planner to move forward instead it just oscillates in its current position.

Possible Solution

Instead of setting a static value for prune_distance we can calculate it dynamically using the below formula:

By scaling this prune_dist based on the current velocity we can avoid the above mentioned issues.

This should also improve the performance of the robot near during turns especially if the robot tends to move to close to the obstacle near sharp turns.

[SteveMacenski]

Did you figure this out?

[SteveMacenski]

Any updates / thoughts on this?

[shrijitsingh99]

I had made some progress on this but after that, I didn't follow up on it. Will try and get more conclusive results by end of the week.

[SteveMacenski]

OK, I'm still not convinced that dynamically pruning is really what you want, but maybe it is. With setting that non-exposed parameter to false and some gain tuning, I think you could get the behavior you desire.

But also keep in mind that if there's a path that goes around something and there's a short cut, a DWA using that short cut might be considered to be good, intended behavior, with goal terms dominant. Its only bad behavior when the path terms are dominant.

[shrijitsingh99]

OK, I'm still not convinced that dynamically pruning is really what you want, but maybe it is. With setting that non-exposed parameter to false and some gain tuning, I think you could get the behavior you desire.

Yeah I too now have come to believe it should be possible with some tuning. Will have to test out and confirm once finally.

Though that parameter should definitely be exposed, its hidden behaviour which isn't good.

[SteveMacenski]

Yeah, I agree with you on that one. I also think that the distances for pruning behind you and in front of you could very reasonably be different and tunable

[SteveMacenski]

@shrijitsingh99 given what you know now - is this still an issue?

[shrijitsingh99]

Closing this since I am not able to get the time to look into this. Everything seems to work fine for now so this may not be required.