working version without visual servoing