Trajectory planning with task constraints in densely filled environments