Merge pull request #213 from ipa-fxm/indigo_dev

Indigo dev
Merge branch 'indigo_dev' into velocity_interface_controller_indigo