Debugging contact constraint for articulated body systems