Change to RigidBodyManipulator contactConstraints.

Motivated by failure of original formulation when used with TaylorVars, changed contactConstraints so that 

phi = <relpos,normal>

instead of

phi = sgn(<relpos,normal>) * ||relpos||

As a note, phi(x0) = 0 resulted in strange behavior on the sqrt() step