No longer do optimize angles in Hierarchical planner
Cleaned up A* intermediate planner using new Wayset code