Fixed bug in RestingContact::solve_lcp and KE gain calculation