added new heuristic for reaching the goal almost exactly | fixed half sitting position at goal