When the change in velocity is exactly the same on several axes, the T1
of the recumputed trajectories after time sync was set to 0 because it
was skipping both if and else if.
Extract the functionality to plan smooth position motion trajectories into a
motion planning library, such that it can be used in other parts of the code as well.
Thomas