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.
Do you think the line 4 (equation 1) in the attached figure should use u1d square?
And then line 10 should be root square of (ConstantFeedrate./vecnorm(r1d, 2))?
Also, do you think the evaluation point should be 1.0 instead of 0.0 in the CutZeroEnd function (line 3):
Thanks a lot for detecting some bugs in our code.
I will make the corresponding gitlab issues.
It seems to me that the comment line 3 of GetCurvPeakAJ should be
% rdot’*rdot = r1D’*r1D * u1d.^2
Line 4 should be
% ConstantFeedrate = sqrt(r1D’*r1D) * u1d
Line 10 should be
u1d = ConstantFeedrate./vecnorm(r1D, 2);
So the bug is that the 1-norm instead of the 2-norm was used.
In CutZeroEnd line 3 should be :
[~, r1D] = EvalCurvStruct(ctx, CurvStruct, 1);
Many thanks and best regards,
I think in the CutZeroEnd function, the second derivative of uk should be -jps * t instead of jps * t (Line 13 in the following figure):
So maybe we should create another function CalcZeroEndConstraints, because Line 7 should also be -jps * t:
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):
Thanks a lot for these valuable comments.
I will analyze and come back to you.