So I improved the arm design. With my modern robotics library this became very very easy.
Although, I forgot to set all the variables correctly. This resulted in a small arm piece to have a mass of 0.5 kg.
Anyway, after I resolved that, the design worked very well. The controllers are reduced to a double proportional controller. The operation is really good.