Closed EdsterG closed 5 years ago
It appears to me that the constraint is violated due to numerical errors. The main issue is probably because the final trajectory is computed as a cubic spline interpolation. The fact that you use a very rapidly varying limit also make the issue more pronouced.
Using a slightly more relaxed check like the one below passes the assertion.
start = np.linalg.norm(qs_sample - way_pts[1], axis=1).argmin()
end = np.linalg.norm(qs_sample - way_pts[2], axis=1).argmin()
assert np.all(np.abs(qds_sample[start+4:end-3]) <= 0.51)
Increasing the number of gridpoints also seems to help. See below for a plot of the computed joint velocity (solid lines) and the bounds (red dashed lines).
When using varying joint velocity constraints, the final trajectory violates some of the constraints.