The trajectory generator is complete and very performant!

The theoretical error is quite small, with fractions of a millimeter. Generation times, especially when done iteratively, are very fast.

First test on the robot: