A unified geometric approach to modeling and control of constrained mechanical systems