Merge pull request #5 from ipa-fxm/indigo_dev

Indigo dev
Merge branch 'indigo_dev' into velocity_interface_controller_indigo