Joint controller updates and testing with herkulex servo