Fixing handling of non-world frame velocities in setModelState.
Adding test for set_model_state
Fixing set model state method and test
Merge pull request #238 from ayrton04/indigo-devel

Fixing handling of non-world frame velocities in setModelState.