Hi,
I don't know what you mean with "the position correction considering the stability". Maybe the following paper can help you:
http://i31www.ira.uka.de/~jbender/Papers/STAR.pdfFor computing the impulses p you must solve the following linear system:
J M^-1 J^T p = dv
where J is the Jacobian of the constraints and M is the mass matrix. For the Lagrange multiplier method you have exactly the same matrix. In contrast to other methods you obtain the right hand side of the system dv by making a preview of the joint state. This solves the drifting problem.
Jan