fixed one case in cost_func, added comment to priority queue (do we need to sort upon update?), added heuristic to compute succ in grid motion graph - in case anyone wants to pass it -, fixed one case in astar cost func, added heuristic to goal node in initialization of dstar
changed update heuristic loop to gridmotiongraph func