Title: Hierarchical control and trajectory planning