Small changes to isGoal and isSafePosition