Motion profile planning for point to point control