Hierarchical Planner now does start/end orientations