Drake function

This implements the "DrakeFunction" idea we'd discussed in #269. I'd like to get feedback about any and all aspects of it, so please, let me know what you think!

This could (probably) be merged in as is without breaking things - everything is contained in the `drakeFunction` package. The only exception is that I had to add the drake root folder to the path in `addpath_drake` in order to get `drakeFunction` on the path.

I will make the change later, once #302 is merged, we cannot call `NonlinearProgramWConstraintObjects` any more, they are all moved to `NonlinearProgram`