Feedback on the contact

Add feedback stabilization on the contact (changes the derivative of the invdyn).