Added unfinished PlanningGraph heuristic