Fixed orientation for non linear movements. Added test sketch. Fixed som...

...e convergence blending issues