Need to Validate Gridpoints Internally

[EdsterG]

The following snippet of code crashes toppra, even though it shouldn't. I understand why it crashes, two of the grid points are extremely close, specifically gp[8] and gp[9]. However, the input is strictly increasing so it shouldn't crash.

import numpy as np

import toppra as tra

waypoints = np.array(
    [[0.016844487206594733, 0.4838585864979808, -0.02150790868407046, -1.7921138616816437, 0.013312268835191154, 0.8655538727538646, -0.010831701942519256],
     [0.12784104717718764, 0.4838631532810903, -0.01997162949235118, -1.7921109502680679, 0.012361392489820378, 0.8655569554312098, 0.10214172868158178],
     [0.23883760714778063, 0.48386772006419965, -0.018435350300631925, -1.7921080388544919, 0.0114105161444496, 0.8655600381085549, 0.2151151593056828],
     [0.3498341671183736, 0.48387228684730904, -0.016899071108912617, -1.792105127440916, 0.010459639799078796, 0.8655631207859, 0.32808858992978396],
     [0.4608307270889665, 0.48387685363041844, -0.015362791917193336, -1.7921022160273403, 0.009508763453708019, 0.8655662034632453, 0.4410620205538849],
     [0.5718272870595593, 0.4838814204135279, -0.013826512725474027, -1.7920993046137643, 0.008557887108337242, 0.8655692861405904, 0.5540354511779859],
     [0.6828238470301524, 0.4838859871966373, -0.012290233533754719, -1.7920963932001885, 0.007607010762966437, 0.8655723688179356, 0.6670088818020871],
     [0.7938204070007453, 0.48389055397974673, -0.010753954342035466, -1.7920934817866125, 0.006656134417595633, 0.8655754514952809, 0.7799823124261881],
     [0.9048169669713383, 0.4838951207628562, -0.009217675150316185, -1.7920905703730368, 0.005705258072224856, 0.8655785341726261, 0.8929557430502891],
     [1.0158135269419313, 0.4838996875459656, -0.007681395958596904, -1.792087658959461, 0.004754381726854079, 0.8655816168499713, 1.0059291736743903],
     [1.126810086912524, 0.483904254329075, -0.006145116766877623, -1.792084747545885, 0.003803505381483302, 0.8655846995273164, 1.1189026042984913],
     [1.2378066468831173, 0.4839088211121844, -0.004608837575158342, -1.7920818361323092, 0.0028526290361124973, 0.8655877822046616, 1.2318760349225926],
     [1.34880320685371, 0.48391338789529387, -0.0030725583834390335, -1.7920789247187334, 0.0019017526907417204, 0.8655908648820068, 1.3448494655466936],
     [1.459799766824303, 0.48391795467840326, -0.0015362791917197527, -1.7920760133051574, 0.0009508763453709435, 0.8655939475593519, 1.4578228961707946],
     [1.570796326794896, 0.4839225214615127, -4.440892098500626e-16, -1.7920731018915816, 1.3877787807814457e-16, 0.8655970302366971, 1.5707963267948957]]
)

path = tra.SplineInterpolator(np.linspace(0, 1, len(waypoints)), waypoints)

vlim_ = np.deg2rad([50, 50, 55, 40, 70, 75, 75])
alim_ = np.deg2rad([150, 150, 150, 150, 150, 150, 150])
pc_vel = tra.constraint.JointVelocityConstraint(np.vstack((-vlim_, vlim_)).T)
pc_acc = tra.constraint.JointAccelerationConstraint(
    np.vstack((-alim_, alim_)).T,
    discretization_scheme=tra.constraint.DiscretizationType.Interpolation
)

gp = np.array([0.0, 0.02040816326530612, 0.04081632653061224, 0.061224489795918366, 0.07142857142857141, 0.08163265306122448, 0.1020408163265306, 0.12244897959183673, 0.14285714285714282, 0.14285714285714285, 0.16326530612244897, 0.18367346938775508, 0.2040816326530612, 0.2142857142857143, 0.22448979591836732, 0.24489795918367346, 0.26530612244897955, 0.28571428571428564, 0.2857142857142857, 0.3061224489795918, 0.32653061224489793, 0.3469387755102041, 0.35714285714285704, 0.36734693877551017, 0.3877551020408163, 0.4081632653061224, 0.42857142857142855, 0.44897959183673464, 0.4693877551020408, 0.4897959183673469, 0.49999999999999994, 0.5102040816326531, 0.5306122448979591, 0.5510204081632653, 0.5714285714285713, 0.5714285714285714, 0.5918367346938775, 0.6122448979591836, 0.6326530612244897, 0.6428571428571428, 0.6530612244897959, 0.673469387755102, 0.6938775510204082, 0.7142857142857142, 0.7346938775510203, 0.7551020408163265, 0.7755102040816326, 0.7857142857142858, 0.7959183673469387, 0.8163265306122448, 0.836734693877551, 0.8571428571428571, 0.8571428571428572, 0.8775510204081632, 0.8979591836734693, 0.9183673469387754, 0.9285714285714286, 0.9387755102040816, 0.9591836734693877, 0.9795918367346939, 1.0])
assert np.all(np.diff(gp) > 0)

instance = tra.algorithm.TOPPRA(
    [pc_vel, pc_acc], path,
    gridpoints=gp,
    solver_wrapper="seidel"
)

jnt_traj, aux_traj = instance.compute_trajectory(0, 0)

assert jnt_traj is not None

[hungpham2511]

Thank for the report. Can you check if this issue persists in the latest commit?

[EdsterG]

I still haven't had the chance to test this completely yet. Hope to do so before next Friday.

[EdsterG]

Thank you! This seems to be resolved now.