Differential flatness u extraction

The complete version of invert flat outputs. Includes some refactoring, the u extraction and the output of actual PPTrajs instead of deprecated functionHandleTrajs.