[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 
Chris, I had to rearrange line 182 here because it couldn't find the necessary "+" operators (on Windows). The expression before required vector+class operators which didn't seem to exist, while class+vector operators did. I don't understand how this was working before.