Closed duanyutong closed 3 years ago
Thanks for the report. I will have a look!
From my inspection it seems because the input path is quite simple, the automatic gridpoint selection procedure returns too few points, thus leading relatively large constraint violation.
Can you try the feat-traj-quality branch to see if there is an improvement?
Btw, there will always be some overshoot due to numerical issues but in general you can increase the number of gridpoints to eliminate the effect.
Thanks for the quick reply. I tried the feat-traj-quality
branch and am getting the same result.
I also tried manually generating more gridpoints using
gridpoints = interpolator.propose_gridpoints(path, max_err_threshold=1e-6, max_iteration=10000)
which provides 1026 gridpoints rather than 33. The limits were still exceeded by O(1)
or 10 percent. Changing to max_err_threshold = 1e-7
further triples the gridpoints but does not make much difference.
If it were only O(1e-2)
I'd take it as numerical issues and be quite happy.
I see, thanks again for the report.
The problem is that in the default setting, the parametrizer is spline-based. Thus there will be vibrations, ... etc, in the final trajectory. The solution is to use a new parametrizer called ConstAccelParametrizer. Could you try to follow the instruction here: https://github.com/hungpham2511/toppra/blob/feat-traj-quality/docs/source/notes.rst#using-different-parametrizers to see if it works for your use case?
For your reference, with this input
# compute trajectory
path = ta.SplineInterpolator(np.linspace(0, 0.15, waypts.shape[0]), waypts)
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(alim)
instance = algo.TOPPRA(
[pc_vel, pc_acc], path,
solver_wrapper="seidel",
parametrizer="ParametrizeConstAccel",
gridpt_min_nb_points=1000
)
I got this output
elements exceeding limit:
[ 15.0179364 15.02476244 -20.03341294 -20.01996555 -20.00222158
20.01718024 20.01087443 20.00650159 15.00011875 -20.00001006
-15.00002658] (array([ 19, 22, 42, 44, 45, 77, 79, 82, 264, 903, 911]), array([0, 0, 5, 5, 5, 5, 5, 5, 0, 5, 0]))
limit:
[ 15. 15. -20. -20. -20. 20. 20. 20. 15. -20. -15.]
And please try the feat-traj-quality branch when you try to run the above code.
Thanks a lot for looking into this. I was able to get your result and will use ParametrizeConstAccel
with feat-traj-quality
branch instead of the default ParametrizeSpline
as the parameteriser choice. I see that you've already updated the docs in that branch.
Thanks in advance for the super useful package. Here's a possible bug I found.
Describe the bug
The given acceleration constraint is not respected, even when input waypoints are smooth without oscillations.
To Reproduce
See minimal breaking example below. I've tried tweaking the upper bound of
path
from 1e-4 to 1, no difference.The output shows that alim is violated for some samples.
Expected behavior
Optimised CubidSpline has second derivative within given limits everywhere.
Screenshots
Smooth input waypoints
Version
master ed07de8