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.
