Choosing when to reject movement

[jimmy-mcelwain]

I have been spending a lot of time looking at MotionControl.c and thinking of potential improvements. I will likely submit a PR at some point that reduces code duplication/makes it a bit more readable. But one thing that I noticed which I've mentioned to @ted-miller is the way that velocity checking is done.

For both FJT and point queue movements, the only the very first point has its velocity checked early enough that it can get rejected if it is not within the limits.

So if say the 100th point of a trajectory has a velocity that exceeds the limits, then FJT will accept it and then abort the goal in the background once the processing thread gets to the 10th point and determines that it is too fast. Maybe this makes sense. I don't know what level of processing should be done for all points in an FJT before accepting vs what should be done in the background after the submission. This seems like it will get especially complicated once we (hopefully) support trajectory blending.

But what makes less sense in my opinion is the fact that if I am using queue traj point mode and I submit my first traj point with a velocity that is too high, I will get a response to the service call saying that the point was rejected. But if I submit an acceptable first traj point, and then afterwards I submit a second traj point with a velocity that is too high, I will get an OK response, but later the movement gets quietly aborted in the background.

This is a pretty specific complaint, but I guess my more general question is what do we want to do when it comes to trajectory validation? Especially, what should we validate "early" (before accepting) and what should we validate "late"? Should we do the same thing for both FJT and the queue traj point service? This is also relevant to some of the discussion in #255, particularly the part about adding some sort of filtering.

[ted-miller]

For FJT, I do think it's better to validate "early" and only accept the action if the entire goal is valid.

For QTP, it absolutely needs to validate every point as they are submitted. A quiet failure is not good.