Update QP controller

It's bad form to have code in the .h files.  But I think it's actually dangerous to have template code in the header files.  Can we move these methods down into controlUtil.cpp?