Visual servo integration fixes