Implementing ContactConstraint for ariticulated rigid bodies