added penalty for directional change in astar planning