Major improvement/rewrite from initial A* implementation
Major improvement/rewrite from initial A* implementation

UM-ARM-Lab/arc_utilities,UM-ARM-Lab/arc_utilities,WPI-ARC/arc_utilities,WPI-ARC/arc_utilities,WPI-ARC/arc_utilities,UM-ARM-Lab/arc_utilities