Revision history [back]

Actually, there is a kind of repulsive force applied on the band. The teb approach deforms the trajectory by solving an optimization problem rather than applying forces directly. There is not much difference, since the solver determines forces implicitly (e.g. gradient of the objective/cost function). The objective function term for obstacle avoidance is just the penalization of the euclidean distance between the obstacle and the robot. Note, obstacle shape and robot shape might differ from points with a much higher computational burden. The optimization problem is defined and implemented in terms of a hyper-graph to speed up sparse Jacobian computation. You can find the corresponding Edge for obstacle avoidance in edge_obstacle.h.

The fundamental algorithm is as follows (you already mentioned the first steps in your question):

1. Initialize or update trajectory
2. Repeat for a number of iterations
   1. If necessary, resize trajectory (similar to insertion and deletion of bubbles in the classical elastic band, but here based on the temporal domain.
   2. Build graph (here you can find which poses are associated with which obstacles, there are two different strategies currently. Ideally, you would connect all obstacles with all poses each, but however, we want to speed up optimization).
   3. Solve optimization problem defined by the previously created graph (stop after some iterations)

The repetition of resizing, obstacle association (in the graph building stage) and some actual optimization steps (3.) leads to the overall band deformation.