Velocity Constraints Not Satisfied

[EdsterG]

When using varying joint velocity constraints, the final trajectory violates some of the constraints.

import numpy as np
import scipy
import toppra as ta

way_pts = [[1, -1, -1, 0, 0],
           [1, 1, -1, 0, 0],
           [-1, 1, -1, 0, 0],
           [-1, 1, 1, 0, 0]]
ws = np.linspace(0, 1, 4)
path = ta.SplineInterpolator(ws, way_pts)

# Create velocity bounds, then velocity constraint object
vlim_ = np.ones(5) * 10
vlim = np.vstack((-vlim_, vlim_)).T
# pc_vel = ta.constraint.JointVelocityConstraint(vlim)
vlim_func = scipy.interpolate.interp1d(ws, [vlim, vlim / 20, vlim, vlim], kind="zero", axis=0)
pc_vel = ta.constraint.JointVelocityConstraintVarying(vlim_func)
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(5) * 100
alim = np.vstack((-alim_, alim_)).T
pc_acc = ta.constraint.JointAccelerationConstraint(
    alim, discretization_scheme=ta.constraint.DiscretizationType.Interpolation)

# Setup a parametrization instance
instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel')

# Retime the trajectory, only this step is necessary.
jnt_traj, _ = instance.compute_trajectory(0, 0)

ts_sample = jnt_traj.ss_waypoints
qs_sample = jnt_traj.eval(ts_sample)
qds_sample = jnt_traj.evald(ts_sample)

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.abs(qds_sample[start:end]) <= 0.5

[hungpham2511]

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