# teb_local_planner: avoid constant path replanning

Hello,

We are working on a heavy omnidirectional platform using orientable wheels. We are using teb_local_planner for navigation. While the overall performance of the planner is good, we are facing serious issues in certain situations, particularly when there are close-by objects.

In these situations, teb planner is constantly re-planning the path, resulting in two different plans alternating quickly.

One of the main characteristics of the orientable wheels is that the platform has a pretty slow reaction to orientation changes (it has to reconfigure the wheel's orientation), so it is not able to react to the path change fast enough. The result is that the robot gets stuck in the position, with the wheels "dancing" between the two required configurations for the two different alternating paths.

Here are a coupe of sample videos (one, two) (rviz of the real robot) in which it can be appreciated. The plan is alternating between two apparently safe paths, but the robot is unable to configure wheels in time to follow a plan before it changes. So it gets stuck and doesn't move.

Here is another video of the robot stuck in a similar situation. However, this other video shows how the robot is able to travel one path in one direction, but gets stuck in the same way in the opposite. As is seen in the video, the robot is actually capable of do it, but the planner is not able to create an adequate plan. When manually setting an additional "waypoint", the robot is able to get out. It can be seen, however, how the robot struggles with alternating re-plans several times.

We have tried to penalize angular acceleration to compensate for the slow reaction of the robot. However, it doesn't seem to have any effect on the creation of alternating plans (which is actually the source of the problem) and higher acceleration limits seems to perform better (lots of not feasible trajectories with very slow angular accelerations).

Setting legacy_obstacle_association to true alleviates greatly the problem. It seems that including all the obstacles in the planning makes the robot to pass further away from obstacles and makes paths more stable. However, the problem persists when the robot needs to pass close to obstacles.

So far we haven't found or guess any way or parameter to force that the planner only changes the current plan under more strict conditions to avoid it alternate between two. Is there such an option? What other parameters can we tune to alleviate this behavior?

EDIT:

When re-planning, there doesn't seem to be any warning or feasibility check fail message, so both plans seems to be valid. The behavior looks like it is alternating between two very close local minima, and the noise from the sensors seems enough to make the planner jump from one minima to the other and back. Sensor noise makes the obstacles around the robot look a bit closer or farther away, which can be enough to trigger the jump ...

edit retag close merge delete

Really interesting behavior which I would really like to fix. Are you still working on it? There should be a warning everytime the planner resets, e.g., feasibility check failed. Can you report me any warning?

( 2018-06-27 01:52:23 -0500 )edit

Yes, still working on it. There doesn't seem to be any warning being displayed. I've added a small edit with more details. Please check it. Next week I'll be working again on it, so I will report any update I find.

( 2018-07-02 02:33:55 -0500 )edit

hmm, according to the video, I cannot really see if the oscillations occur from multiple trajectories or not. In that case, this should not happen if homotopy class planning is disabled.

( 2018-07-05 09:03:50 -0500 )edit
1

However, I followed your advice regarding the switching behavior. But instead of switching only if the current trajectory is infeasible (which would take a little bit more effort to implement) I added a new parameter switching_blocking_period (see wiki page) in the new release I pushed today.

( 2018-07-05 09:06:26 -0500 )edit

Sort by » oldest newest most voted

Maybe you can improve the value of selection_cost_hysteresis to prevent the switching between the paths which have very similar cost.

more