Fanuc R2000iA-200F Robot with linuxcnc and odrives

After quite the long journey, it finally (mostly) works.
I still have some issues to fix, mostly with the PID tuning, I'm having a hard time getting the following error to be acceptable but also not get oscillations. I'm thinking an issue is that I am updating the motor torque at only 300hz. What I find strange is that I only get oscillations while moving (large low frequency oscillations), it settles out when stopped just fine.

GitHub with documentation and all the custom files/firmware: github.com/ExcessiveOverkill/universal-IRC-1
Video with explanation of whole project: 

Big thanks to PCW and andypugh, along with anyone else who helped out answering all the questions I came up with.

Very nice project!

In this video Yaskawa recommends 4x bandwidth separation between the three control loops to maintain stability, so that leaves, what, 20 Hz bandwidth for your position loop?

Edit: Sorry, I'm confusing bandwidth and update rate here. 300 Hz torque loop update rate means the best torque bandwidth you could hope for in an ideal system is 150 Hz (Nyquist criteria), which leaves less than 10 Hz of bandwidth for your position loop. I'm just saying it's probably more useful right now to spend time working on your custom drivers (or otherwise increasing the torque update rate) rather than trying to minimise following error by tuning.

Currently the plan is for it to be a CNC router(if the backlash is low enough), or plasma cutter. Both of those depend quite heavily on getting the following error better however.

I'm going try to get the torque update speed to 1khz and see if there is any major improvement. If there is then at least I know that's probably the limiting factor.

The custom drives are in progress, but I am expecting them to take quite a while to get fully functional and reliable.