[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 
> By the way, @sherm1 are you okay with my use of this macro?

Well, it's ugly and will make stepping around in the debugger impossible. I am sympathetic about not wanting to write the same code over and over. My thought is that this could be done using a template class instead of a macro though.