Good job ChristianLL, but how about writing your kinematics as plugin for Mach, something like this guy is showing in his THREAD.