Nearly smooth now. No Timeouts anymore.
Using now moveRobotToEnc4D with the last spline position.
Made joint no 4 faster
removed timeouts
Made joint 3 faster
Commented making the joints faster out.
Made joint 2 faster and more acceleration. WORKS!!!111einseinseins