Updated SustainedUnilateralConstraintHandler with reference mode for inertia matrix calculations