Motion planning in structured environments