A focus servo system with a focus close algorithm is disclosed. The focus close algorithm provides a bias control effort to the focus servo system when focus is closed. A method of closing focus includes positioning an optical pick-up unit at a first position away from an optical medium, moving the optical pick-up unit towards a second position by altering a focus control effort, monitoring a sum signal obtained from signals received from the optical pick-up unit, and setting the bias control effort to the focus control effort when a closure criteria is satisfied. In some embodiments, the closure criteria includes that a sum signal exceeds a threshold value. In some embodiments, the closure criteria includes that a focus error signal is below a FES threshold.