Motion planning with collisions