Hierarchical solution of non-convex optimal control problems with application to autonomous driving.