Grid cost initialization now works

Also adding grid cell cost based on distance to goal.