A numerical method for the servo constraint problem of underactuated mechanical systems. Issue 1 (21st October 2015)