Fix #100: Correction for constraint errors

This pull request slightly modifies the LCP formulation so that additional velocities in constraint space correct constraint errors.