Now only adding one contact constraint to constraint set