New functions added
New functions added
Cleaned up version
Cleaned up version
Merge branch 'velocity_interface_controller_indigo' of github.com:ipa-fxm-cm/cob_control into indigo_dev