[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 
See here for example: https://github.com/simbody/simbody/pull/238/files#diff-4a11b7723285405eb4f615149d10f340R355