Very cool! Glad you could get it working. It looks like you milled the PCBs yourself? If that is the case that is awesome!
I do highly recommend you investigate PlatformIO, it will make developing for Arduinos a much calmer experience to say the least 
Also, this could be nothing, but in that video it looked to me like the motors were cogging a bit? I don’t think that 3D printed arm was so heavy it would cause that. Would your input voltage happen to be lower than what you set it to be in SimpleFOC? Perhaps I am seeing things, or it is an artifact of your PID terms, but it looked to behave a little wonky to me. Maybe get a second opinion from @runger.
Nevertheless, cool project, glad to see you got it done in time, welcome to the FOC revolution!