Hmm, irgendwie zittern und zappeln die Servos teilweise noch stark. Ich denke mal, dass da irgendwas das Signal stört, aber was es ist musst du wohl selber herausfinden.
Die inverse Kinematik funktioniert ja aber schon ziemlich gut, hast du auch schon andere Bewegungen probiert? Also zB. seitwärts, vertikal oder verkippen im Raum.