Extension of a Penalty Method for Numerically Solving Constrained Multibody Dynamic Problems