'Parametric B-spline interpolation in N-dimensions.
Parameters
traj : array_like (float)
Solution trajectory providing the data points for constructing the
B-spline representation.
ti : array_like (float)
Array of values for the independent variable at which to
interpolate the value of the B-spline.
k : int, optional(default=3)
Degree of the desired B-spline. Degree must satisfy
:math:`1 \le k \le 5`.
der : int, optional(default=0)
The order of derivative of the spline to compute (must be less
than or equal to `k`).
ext : int, optional(default=2) Controls the value of returned elements
for outside the original knot sequence provided by traj. For
extrapolation, set `ext=0`; `ext=1` returns zero; `ext=2` raises a
`ValueError`.
Returns
interp_traj: ndarray (float)
The interpolated trajectory.'

def interpolate(self, traj, ti, k=3, der=0, ext=2):

    u = traj[:, 0]
    n = traj.shape[1]
    x = [traj[:, i] for i in range(1, n)]
    (tck, t) = interpolate.splprep(x, u=u, k=k, s=0)
    out = interpolate.splev(ti, tck, der, ext)
    interp_traj = np.hstack((ti[:, np.newaxis], np.array(out).T))
    return interp_traj
