Correct number of DOFs with constraints

This should fix the problem with counting the number of degrees of freedom with constraints while also keeping the fix introduced earlier for PIMD.

Needs testing and review.