acceleration constraint is not respected

[duanyutong]

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.

import numpy as np
import toppra as ta
import toppra.algorithm as algo
from toppra import constraint
import matplotlib.pyplot as plt

# input data
waypts = np.array([[-0.64142144, -1.48907971,  1.894104  , -2.2074306 ,  1.92707002,
         1.50737071,  1.20313358],
       [-0.64145738, -1.48909926,  1.89408648, -2.20745182,  1.92706716,
         1.50738573,  1.20313358],
       [-0.64145738, -1.48909926,  1.89408648, -2.20745182,  1.92706716,
         1.50738573,  1.20313358],
       [-0.64142966, -1.48908424,  1.89409995, -2.20743537,  1.92706931,
         1.50737417,  1.20313358],
       [-0.63920563, -1.48959816,  1.89454412, -2.2084074 ,  1.9284476 ,
         1.51026404,  1.20470095],
       [-0.63463145, -1.49070168,  1.89595389, -2.21121669,  1.93131077,
         1.51618505,  1.2078774 ],
       [-0.6278106 , -1.49234819,  1.89800382, -2.2152307 ,  1.93571103,
         1.52499902,  1.21233833],
       [-0.61042947, -1.49643397,  1.90333021, -2.22506738,  1.94692171,
         1.54750407,  1.22376776],
       [-0.57696438, -1.50457776,  1.91456592, -2.24278426,  1.96783555,
         1.59136438,  1.24697554],
       [-0.54576355, -1.51244676,  1.92596698, -2.25792789,  1.98712289,
         1.63321686,  1.26982975],
       [-0.52902359, -1.51683176,  1.93251836, -2.26542211,  1.99711704,
         1.65592957,  1.28268933],
       [-0.52094024, -1.51897228,  1.93576336, -2.26886582,  2.00186777,
         1.66694438,  1.28901827],
       [-0.51691443, -1.52007115,  1.9374156 , -2.27054   ,  2.0042026 ,
         1.67247248,  1.29224515],
       [-0.51555175, -1.5204109 ,  1.93802011, -2.27124834,  2.00526309,
         1.67445993,  1.29325616]])
vlim = np.array([[-2.175,  2.175],
       [-2.175,  2.175],
       [-2.175,  2.175],
       [-2.175,  2.175],
       [-2.61 ,  2.61 ],
       [-2.61 ,  2.61 ],
       [-2.61 ,  2.61 ]])
alim = np.array([[-15. ,  15. ],
       [ -7.5,   7.5],
       [-10. ,  10. ],
       [-12.5,  12.5],
       [-15. ,  15. ],
       [-20. ,  20. ],
       [-20. ,  20. ]])
fig, ax = plt.subplots(figsize=(10, 6))
ax.plot(waypts, 'o-')

# 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")
jnt_traj = instance.compute_trajectory(0, 0)

# check alim
deriv = jnt_traj.evaldd(cs.x)
i, j = np.where(~((alim[:, 0] < deriv) & (deriv < alim[:, 1])))
signed_lim = np.where(deriv > 0, alim[:, 1], alim[:, 0])
print(f'elements exceeding limit:\n{deriv[i, j]}')
print(f'limit:\n{signed_lim[i, j]}')

The output shows that alim is violated for some samples.

elements exceeding limit:
[ 15.65610175  20.94790152  18.67345754  25.09828735  17.19680362
 -14.31188803  22.06135898  15.35339847 -15.57742397 -20.01138662
 -20.19835481]
limit:
[ 15.   20.   15.   20.   15.  -12.5  20.   15.  -15.  -20.  -20. ]

Expected behavior

Optimised CubidSpline has second derivative within given limits everywhere.

Screenshots

Smooth input waypoints

Version

master ed07de8

[hungpham2511]

Thanks for the report. I will have a look!

[hungpham2511]

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.

[duanyutong]

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.

[hungpham2511]

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.]

[hungpham2511]

And please try the feat-traj-quality branch when you try to run the above code.

[duanyutong]

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.