The current path planning makes it that there is an infinite acceleration required in the corners. We need something that can process setpoints in a way that it can follow those.
So if we want to draw a box. 4 setpoints for each corner should be sufficient. This is a simple model detail. We just take the four setpoint and give the system some time to get there.