I’m not quite sure what are the two functions for? I guess they have something to do with the constant jerk strategy.
Within the function, the jps (third derivative of s) is computed from:
jps = min(cfg.jmax) / max(abs(r1D));
Is it an estimation of the constant jerk, because the exact relation is:
Also in the function GetCurvPeakAJ, in line 3, the dot product is computed as:
rdot’*rdot = r1D’*r1D * u1d, but from line 2, it should be:
rdot’*rdot = r1D’*r1D * u1d^2 (2)
In line 10, from (2), should the denominator be vecnorm(r1D, 2) instead of vecnorm(r1D, 1), and then take the square root of ConstantFeedrate./vecnorm(r1D, 2) to get the u1d.
Also, u2d and u3d is zero. Is this assumption valid for spline curve? because the first derivative norm may not be constant.
indeed the functions CutZeroStart and CutZeroEnd are related to the constant pseudo jerk strategy at zero speed. If you have a geometrical curve piece, you can split it up into several pieces. So the first (short) piece of curve will happen at constant pseudo jerk.
For the initial guess of pseudo jerk we neglected the curvature.
If the value is too high, it will be reduced by a factor in a while loop, until the constraints are met.
If the value is too small we keep it, knowing that the start phase at constant pseudo jerk is typically very short, so there is not a big sacrifice in time optimality.
Indeed, for a spline, u2d and u3d are typically different from zero and they vary.
But for simplicity reasons, we neglected the parametric derivatives higher than 1.
For sure, there might be better strategies to find a reasonable value for the initial guess of pseudo jerk.
Another issue is after halving the jerk, it seems the new jerk was not set back. So in the evaluation stage, we still use the old jerk and it may cause violation of acceleration around zero speed. Maybe we need something like this (Line 171 - 173 and Line 181 - 183):
