Adapt teached position as function of deviation 3-D, 2-D position of end effector, tool