Как мне кажется проблема в настройке привода или датчика положения по Z. Была несколько иная ситуация :

при движении по пологой траектории происходила раскачка по Z. То есть при движении на подаче 300 мм/мин при движении одновременно по X 150мм и по Z 3 мм начинались колебания по Z. Доходило до "пилы" амплитудой до миллиметра, а то и больше...

Решили настройкой жесткости на плате привода.(если ничего не путаю).

Плюс была проблема с некачественным маслом в системе смазки. Досталось А20, которое было совсем не А20 )) Специалист пощупав пальцами то масло сказал что оно "не скользкое совсем", теперь лью М8В.

В общем после всех приключений остается замирание на траектории при появлении в кадре Z...