Fixed bug in contact minimization, resting contact handling