Joint following error

Setting the ferror will have no effect on latency issues
I think its just luck.

I would try a 2 ms servo thread
(this also requires setting all the PID P terms to 500)