Updated VisualServo