Fixed optimization bug
Fixed optimization bug

Velocity smoothing introduced a bug in optimization, where the peak
velocity of the last segment on the queue changes when a new tangent
move is added. The original algorithm trusts that these velocities are
constant, so it overestimated how fast the previous segment could go,
causing acceleration violations.

The simple fix is just to start 1 deeper in the queue. Effectively, the
last move on the queue is in "exact stop" mode until a new move is
added.

ArcEye/machinekit-testing,araisrobo/machinekit,ArcEye/machinekit-testing,ArcEye/MK-Qt5,araisrobo/machinekit,ArcEye/MK-Qt5,araisrobo/machinekit,strahlex/machinekit,ArcEye/MK-Qt5,bobvanderlinden/machinekit,RunningLight/machinekit,Cid427/machinekit,bobvanderlinden/machinekit,EqAfrica/machinekit,EqAfrica/machinekit,unseenlaser/machinekit,ArcEye/MK-Qt5,unseenlaser/machinekit,unseenlaser/machinekit,Cid427/machinekit,EqAfrica/machinekit,RunningLight/machinekit,RunningLight/machinekit,ArcEye/machinekit-testing,bobvanderlinden/machinekit,ArcEye/machinekit-testing,strahlex/machinekit,ArcEye/MK-Qt5,EqAfrica/machinekit,EqAfrica/machinekit,strahlex/machinekit,kinsamanka/machinekit,mhaberler/machinekit,araisrobo/machinekit,strahlex/machinekit,mhaberler/machinekit,RunningLight/machinekit,EqAfrica/machinekit,mhaberler/machinekit,Cid427/machinekit,araisrobo/machinekit,mhaberler/machinekit,bobvanderlinden/machinekit,bobvanderlinden/machinekit,Cid427/machinekit,Cid427/machinekit,ArcEye/machinekit-testing,kinsamanka/machinekit,ArcEye/machinekit-testing,araisrobo/machinekit,ArcEye/machinekit-testing,araisrobo/machinekit,bobvanderlinden/machinekit,kinsamanka/machinekit,RunningLight/machinekit,strahlex/machinekit,unseenlaser/machinekit,ArcEye/MK-Qt5,unseenlaser/machinekit,ArcEye/machinekit-testing,RunningLight/machinekit,ArcEye/MK-Qt5,araisrobo/machinekit,bobvanderlinden/machinekit,unseenlaser/machinekit,kinsamanka/machinekit,mhaberler/machinekit,mhaberler/machinekit,bobvanderlinden/machinekit,araisrobo/machinekit,EqAfrica/machinekit,kinsamanka/machinekit,unseenlaser/machinekit,Cid427/machinekit,RunningLight/machinekit,mhaberler/machinekit,kinsamanka/machinekit,RunningLight/machinekit,Cid427/machinekit,unseenlaser/machinekit,EqAfrica/machinekit,ArcEye/MK-Qt5,mhaberler/machinekit,kinsamanka/machinekit,kinsamanka/machinekit,strahlex/machinekit,strahlex/machinekit,Cid427/machinekit