PbdRbd Grasping Fixes, Fix PbdBodyToBodyDistanceConstraint, Other Cleanups

Now that all PbdCollisionConstraints are solved in the same spot a couple of spots like grasping and stitching started to try to run the correctVelocity step for friction and restitution, which was invalid in some cases. This adds a check on lambda to verify whether the constraint was even applied at all. Particularly when the constraint did not run (return false or c=0) this would produce nans and grasping would fail.