Доброго.
После снятия карты высот перемещаем в X0 Y0 (откуда начинали). Потом обнулить Z, Z-щуп и стартовать УП фрезеровки?