[WIP] Task space feature

This PR continues PR #210 but with the source moved into the simbody/simbody repo from chrisdembia/simbody. (via short-lived PR #237). This is so Chris and I can work on it together and with osrf.

cc/ @chrisdembia @hsu @scpeters 
I was looking online about how to use macros to generate code that one could then paste back into the code file. Some site said to use gcc's preprocessor (`gcc -E ...`). I'm happy to write it all out once/if we decide it's what we want.