Exemplo n.º 1
0
def deriv_bspline3D(order, parameter, u, course, k):
    xd = BSpline(u, course.x, k)
    yd = BSpline(u, course.y, k)
    zd = BSpline(u, course.z, k)

    deriv_xd = xd.derivative(order)
    deriv_yd = yd.derivative(order)
    deriv_zd = zd.derivative(order)

    deriv_bspline_x = deriv_xd(parameter)
    deriv_bspline_y = deriv_yd(parameter)
    deriv_bspline_z = deriv_zd(parameter)

    deriv_bspline_course = CourseClass(deriv_bspline_x, deriv_bspline_y,
                                       deriv_bspline_z)
    return deriv_bspline_course
Exemplo n.º 2
0
def Q_complexity(glmv):

    #take log of V and S
    v = [math.log(i[0]) for i in glmv]
    s = [math.log(i[1]) for i in glmv]

    #find limits for integral
    Smax = max(s)
    Smin = min(s)

    #reverse to increasing order for Bspline()
    v.reverse()
    s.reverse() 

    #convert to arrays for splrep
    x = np.array(s)
    y = np.array(v)

    #find vector knots for Bspline
    t, c, k = interpolate.splrep(x=x, y=y, s= 0, k=4)
    spl = BSpline(t, c, k)
    
    dvds = BSpline.derivative(spl)

    integral = quad(lambda s: ramp(1-(0.25*dvds(s)**2)), Smin, Smax)

    Q = (1/(Smax-Smin))*integral[0] #eq5   

    return Q
Exemplo n.º 3
0
    def buildDerivativeBSpline(self, u, k, order):
        xd = BSpline(u, self.course.x, k)
        yd = BSpline(u, self.course.y, k)
        zd = BSpline(u, self.course.z, k)

        deriv_xd = xd.derivative(order)
        deriv_yd = yd.derivative(order)
        deriv_zd = zd.derivative(order)

        deriv_bspline_course = CourseClass()
        parameter = np.linspace(0, 1, self.n_waypoints)
        deriv_bspline_course.x = deriv_xd(parameter)
        deriv_bspline_course.y = deriv_yd(parameter)
        deriv_bspline_course.z = deriv_zd(parameter)

        return deriv_bspline_course
Exemplo n.º 4
0
def create_spline_basis(
    x, knot_list=None, num_knots=None, degree: int = 3, add_intercept=True
):
    """
    Creates a spline basis functions.

    :param x: array of size num time steps
    :param knot_list: indices of knots
    :param num_knots: number of knots
    :param degree: degree of the spline function
    :param add_intercept: appends an additional column of ones, defaults
        to False
    :return: list of knots, basis functions
    :rtype: Tuple[np.ndarray, np.ndarray]
    """
    assert ((knot_list is None) and (num_knots is not None)) or (
        (knot_list is not None) and (num_knots is None)
    ), "Define knot_list OR num_knot"
    if knot_list is None:
        knot_list = np.quantile(x, q=np.linspace(0, 1, num=num_knots))
    else:
        num_knots = len(knot_list)

    knots = np.pad(knot_list, (degree, degree), mode="edge")
    B0 = BSpline(knots, np.identity(num_knots + 2), k=degree)
    # B0 = BSpline(knot_list, np.identity(num_knots), k=degree)
    B = B0(x)
    Bdiff = B0.derivative()(x)

    if add_intercept:
        B = np.hstack([np.ones(B.shape[0]).reshape(-1, 1), B])
        Bdiff = np.hstack([np.zeros(B.shape[0]).reshape(-1, 1), Bdiff])

    return knot_list, np.stack([B, Bdiff])
Exemplo n.º 5
0
    def get_spline_basis(self):
        self.knots = np.pad(self.knot_list, (self.degree, self.degree), mode="edge")
        B0 = BSpline(self.knots, np.identity(len(self.knots)), k=self.degree)
        self.B_pad = B0(self.t)
        self.B = self.B_pad[self.padding : -self.padding]

        if self.degree > 0:
            self.B_diff = B0.derivative()(self.t)[self.padding : -self.padding]
        else:
            self.B_diff = np.zeros(self.B.shape)[self.padding : -self.padding]
Exemplo n.º 6
0
def deriv_bspline_position(order, position, parameter, u, course, k):
    xd = BSpline(u, course.x, k)
    yd = BSpline(u, course.y, k)
    zd = BSpline(u, course.z, k)

    deriv_xd = xd.derivative(order)
    deriv_yd = yd.derivative(order)
    deriv_zd = zd.derivative(order)

    deriv_parameter = []
    for i in range(0, len(position)):
        deriv_parameter.append(parameter[position[i]])

    deriv_bspline_x = deriv_xd(deriv_parameter)
    deriv_bspline_y = deriv_yd(deriv_parameter)
    deriv_bspline_z = deriv_zd(deriv_parameter)

    deriv_bspline_course = CourseClass(deriv_bspline_x, deriv_bspline_y,
                                       deriv_bspline_z)
    return deriv_bspline_course
def B(order, knots, number, space, deriv):
    c = zeros(len(knots))
    c[number - 1] = 1
    Bspl = BSpline(knots, c, order - 1, extrapolate=False)
    if deriv == 0:
        rBspl = Bspl(space)
    else:
        Bspl = Bspl.derivative(deriv)
        rBspl = Bspl(space)

    nanIdx = isnan(rBspl)
    rBspl[nanIdx] = 0
    return rBspl
Exemplo n.º 8
0
def create_spline_basis(x,
                        knot_list=None,
                        num_knots=None,
                        degree=3,
                        add_intercept=True):
    assert ((knot_list is None) and (num_knots is not None)) or (
        (knot_list is not None) and
        (num_knots is None)), "Define knot_list OR num_knot"
    if knot_list is None:
        knot_list = jnp.quantile(x, q=jnp.linspace(0, 1, num=num_knots))
    else:
        num_knots = len(knot_list)

    knots = jnp.pad(knot_list, (degree, degree), mode="edge")
    B0 = BSpline(knots, jnp.identity(num_knots + 2), k=degree)
    B = B0(x)
    Bdiff = B0.derivative()(x)
    if add_intercept:
        B = jnp.hstack([jnp.ones(B.shape[0]).reshape(-1, 1), B])
        Bdiff = jnp.hstack([jnp.zeros(B.shape[0]).reshape(-1, 1), Bdiff])
    return knot_list, B, Bdiff
Exemplo n.º 9
0
def torsion(
    x: np.ndarray,
    t: np.ndarray,
    c: np.ndarray,
    k: np.integer,
    aux_outputs: bool = False,
) -> np.ndarray:
    r"""Compute the torsion of a B-Spline.

    The torsion measures the failure of a curve, `r(u)`, to be planar.
    If the curvature `k` of a curve is not zero, then the torsion is defined as

    .. math::

        \tau = -n \cdot b',

    where `n` is the principal normal vector, and `b'` the derivative w.r.t. the
    arc length `s` of the binormal vector.

    The torsion can also be computed as

    .. math::
        \tau = \lvert r'(t), r''(t), r'''(t) \rvert / \lVert r'(t) \times r''(t) \rVert^2,

    where `r(u)` is the position vector as a function of time.

    Arguments:
        x: A `1xL` array of parameter values where to evaluate the curve.
            It contains the parameter values where the torsion of the B-Spline will
            be evaluated. It is required to be non-empty, one-dimensional, and
            real-valued.
        t: A `1xm` array representing the knots of the B-spline.
            It is required to be a non-empty, non-decreasing, and one-dimensional
            sequence of real-valued elements. For a B-Spline of degree `k`, at least
            `2k + 1` knots are required.
        c: A `dxn` array representing the coefficients/control points of the B-spline.
            Given `n` real-valued, `d`-dimensional points ::math::`x_k = (x_k(1),...,x_k(d))`,
            `c` is the non-empty matrix which columns are ::math::`x_1^T,...,x_N^T`. For a
            B-Spline of order `k`, `n` cannot be less than `m-k-1`.
        k: A non-negative integer representing the degree of the B-spline.

    Returns:
        torsion: A `1xL` array containing the torsion of the B-Spline evaluated at `x`

    References:
    .. [1] Máté Attila, The Frenet–Serret formulas.
        http://www.sci.brooklyn.cuny.edu/~mate/misc/frenet_serret.pdf
    """

    # convert arguments to desired type
    x = np.ascontiguousarray(x)
    t = np.ascontiguousarray(t)
    c = np.ascontiguousarray(c)
    k = operator.index(k)

    if k < 0:
        raise ValueError("The order of the spline must be non-negative")

    check_type(t, np.ndarray)
    t_dim = t.ndim
    if t_dim != 1:
        raise ValueError("t must be one-dimensional")
    if len(t) == 0:
        raise ValueError("t must be non-empty")
    check_iterable_type(t, (np.integer, np.float))
    if (np.diff(t) < 0).any():
        raise ValueError("t must be a non-decreasing sequence")

    check_type(c, np.ndarray)
    c_dim = c.ndim
    if c_dim > 2:
        raise ValueError("c must be 2D max")
    if len(c.flatten()) == 0:
        raise ValueError("c must be non-empty")
    if c_dim == 1:
        check_iterable_type(c, (np.integer, np.float))
        # expand dims so that we can cycle through a single dimension
        c = np.expand_dims(c, axis=0)
    if c_dim == 2:
        for d in c:
            check_iterable_type(d, (np.integer, np.float))
    n_dim = len(c)

    check_type(x, np.ndarray)
    x_dim = x.ndim
    if x_dim != 1:
        raise ValueError("x must be one-dimensional")
    if len(x) == 0:
        raise ValueError("x must be non-empty")
    check_iterable_type(x, (np.integer, np.float))
    L = len(x)

    # evaluate first, second, and third derivatives
    # deriv, dderiv, ddderiv are (d, L) arrays
    deriv = np.empty((n_dim, L))
    dderiv = np.empty((n_dim, L))
    ddderiv = np.empty((n_dim, L))
    for i, dim in enumerate(c):
        spl = BSpline(t, dim, k)
        deriv[i, :] = spl.derivative(nu=1)(x) if k - 1 >= 0 else np.zeros(L)
        dderiv[i, :] = spl.derivative(nu=2)(x) if k - 2 >= 0 else np.zeros(L)
        ddderiv[i, :] = spl.derivative(nu=3)(x) if k - 3 >= 0 else np.zeros(L)
    # transpose derivs
    deriv = deriv.T
    dderiv = dderiv.T
    ddderiv = ddderiv.T

    cross = np.cross(deriv, dderiv)

    # Could be more efficient by only computing dot products of corresponding rows
    num = np.diag((cross @ ddderiv.T))
    denom = np.linalg.norm(cross, axis=1)**2

    torsion = np.nan_to_num(num / denom)

    if aux_outputs == True:
        return torsion, deriv, dderiv, ddderiv
    else:
        return torsion
Exemplo n.º 10
0
def speed(
    x: np.ndarray,
    t: np.ndarray,
    c: np.ndarray,
    k: np.integer,
    aux_outputs: bool = False,
) -> np.ndarray:
    r"""Compute the speed of a B-Spline.

    The speed is the norm of the first derivative of the B-Spline.

    Arguments:
        x: A `1xL` array of parameter values where to evaluate the curve.
            It contains the parameter values where the speed of the B-Spline will
            be evaluated. It is required to be non-empty, one-dimensional, and
            real-valued.
        t: A `1xm` array representing the knots of the B-spline.
            It is required to be a non-empty, non-decreasing, and one-dimensional
            sequence of real-valued elements. For a B-Spline of degree `k`, at least
            `2k + 1` knots are required.
        c: A `dxn` array representing the coefficients/control points of the B-spline.
            Given `n` real-valued, `d`-dimensional points ::math::`x_k = (x_k(1),...,x_k(d))`,
            `c` is the non-empty matrix which columns are ::math::`x_1^T,...,x_N^T`. For a
            B-Spline of order `k`, `n` cannot be less than `m-k-1`.
        k: A non-negative integer representing the degree of the B-spline.

    Returns:
        speed: A `1xL` array containing the speed of the B-Spline evaluated at `x`

    References:
    .. [1] Kouba, Parametric Equations.
        https://www.math.ucdavis.edu/~kouba/Math21BHWDIRECTORY/ArcLength.pdf
    """

    # convert arguments to desired type
    x = np.ascontiguousarray(x)
    t = np.ascontiguousarray(t)
    c = np.ascontiguousarray(c)
    k = operator.index(k)

    if k < 0:
        raise ValueError("The order of the spline must be non-negative")

    check_type(t, np.ndarray)
    t_dim = t.ndim
    if t_dim != 1:
        raise ValueError("t must be one-dimensional")
    if len(t) == 0:
        raise ValueError("t must be non-empty")
    check_iterable_type(t, (np.integer, np.float))
    if (np.diff(t) < 0).any():
        raise ValueError("t must be a non-decreasing sequence")

    check_type(c, np.ndarray)
    c_dim = c.ndim
    if c_dim > 2:
        raise ValueError("c must be 2D max")
    if len(c.flatten()) == 0:
        raise ValueError("c must be non-empty")
    if c_dim == 1:
        check_iterable_type(c, (np.integer, np.float))
        # expand dims so that we can cycle through a single dimension
        c = np.expand_dims(c, axis=0)
    if c_dim == 2:
        for d in c:
            check_iterable_type(d, (np.integer, np.float))
    n_dim = len(c)

    check_type(x, np.ndarray)
    x_dim = x.ndim
    if x_dim != 1:
        raise ValueError("x must be one-dimensional")
    if len(x) == 0:
        raise ValueError("x must be non-empty")
    check_iterable_type(x, (np.integer, np.float))
    L = len(x)

    # evaluate first and second derivatives
    # deriv, dderiv are (d, L) arrays
    deriv = np.empty((n_dim, L))
    for i, dim in enumerate(c):
        spl = BSpline(t, dim, k)
        deriv[i, :] = spl.derivative(nu=1)(x) if k - 1 >= 0 else np.zeros(L)
    # tranpose deriv
    deriv = deriv.T

    speed = np.linalg.norm(deriv, axis=1)
    if aux_outputs == False:
        return speed
    else:
        return speed, deriv
Exemplo n.º 11
0
class SmoothingSpline(object):
    """ Main cubic smoothing spline class. """
    def __init__(self, x, y, lam=10., max_knots=250, order=3):
        """ Initialize and fit the cubic smoothing spline, using the Bspline basis from
        scipy.

        x: A numpy array of univariate independent variables.
        y: A numpy array of univariate response variables.
        lam: smoothing parameter
        max_knots: max number of knots (i.e. max number of spline basis functions)."""

        ## Start by storing the data
        assert len(x) == len(
            y
        ), "Independent and response variable vectors must have the same length."
        self.x = x
        self.y = y
        self.order = 3
        self.lam = lam

        ## Then compute the knots, refactoring to evenly
        ## spaced knots if the number of unique values is too
        ## large.
        self.knots = np.sort(np.unique(self.x))
        if len(self.knots) > max_knots:
            self.knots = np.linspace(self.knots[0], self.knots[-1], max_knots)

        ## Construct the spline basis given the knots
        self._aug_knots = np.hstack([
            self.knots[0] * np.ones((order, )), self.knots,
            self.knots[-1] * np.ones((order, ))
        ])
        self.splines = [
            BSpline(self._aug_knots, coeffs, order)
            for coeffs in np.eye(len(self.knots) + order - 1)
        ]

        ## Finally, with the basis functions, we can fit the
        ## smoother.
        self._fit()

    def _fit(self):
        """ Subroutine for fitting, called by init. """

        ## Start the fit procedure by constructing the matrix B
        B = np.array([sp(self.x) for sp in self.splines]).T

        ## Then, construct the penalty matrix by first computing second derivatives
        ## on the knots and then approximating the integral of second derivative products
        ## with the trapezoid rule.
        d2B = np.array([sp.derivative(2)(self.knots) for sp in self.splines])
        weights = np.ones(self.knots.shape)
        weights[1:-1] = 2.
        Omega = np.dot(d2B, np.dot(np.diag(weights), d2B.T))

        ## Finally, invert the matrices to construct values of interest.
        self.ridge_op = np.linalg.inv(np.dot(B.T, B) + self.lam * Omega)

        ## From there, we can compute the coefficient matrix H and the
        ## S matrix (i.e. the hat matrix).
        H = np.dot(self.ridge_op, B.T)
        self.S = np.dot(B, H)
        self.edof = np.trace(self.S)
        self.gamma = np.dot(H, self.y)

        ## Finally, construct the smoothing spline
        self.smoother = BSpline(self._aug_knots, self.gamma, self.order)

        ## And compute the covariance matrix of the coefficients
        y_hat = self.smoother(self.x)
        self.rss = np.sum((self.y - y_hat)**2)
        self.var = self.rss / (len(self.y) - self.edof)
        self.cov = self.var * self.ridge_op

        return

    def __call__(self, x, cov=False):
        """ Evaluate it """

        ## if you want the covariance matrix
        if cov:

            ## Set up the matrix of splines evaluations and similarity transform the
            ## covariance in the coefficients
            F = np.array([BSpline(self._aug_knots, g, self.order)(x) \
                          for g in np.eye(len(self.knots) + self.order - 1)]).T
            covariance_matrix = np.dot(F, np.dot(self.cov, F.T))

            ## Evaluate the mean using the point estimate
            ## of the coefficients
            point_estimate = np.dot(F, self.gamma)

            return point_estimate, covariance_matrix

        else:
            return self.smoother(x)

    def derivative(self, x, degree=1):
        """ Evaluate the derivative of degree = degree. """

        return self.smoother.derivative(degree)(x)

    def correlation_time(self):
        """ Use the inferred covariance matrix to compute and estimate of the correlation time
        by approximating the width of correlation for a central knot with it's neighbors. """

        ## Select a central knot
        i_mid = int(len(self.knots) / 2)

        ## Compute a normalized distribution
        distribution = np.abs(self.cov[i_mid][1:-1])
        distribution = distribution / trapz(distribution, x=self.knots)

        ## Compute the mean and variance
        avg = self.knots[i_mid - 1]
        var = trapz(distribution * (self.knots**2), x=self.knots) - avg**2

        return np.sqrt(var)
class TrajectoryGenerator:
    def __init__(self):
        pass

    def create_sin_traj(self, period, amplitude, phase, offset):
        self.type = "sinusoid"
        self.period = period
        self.amplitude = amplitude
        self.phase = phase
        self.offset = offset

    def create_bspline_traj(self, degree, knots, coeffs):
        self.type = "bspline"
        self.knots = knots
        self.x_spline = BSpline(knots, coeffs[0, :], degree)
        self.z_spline = BSpline(knots, coeffs[1, :], degree)
        self.x_derivs = []
        self.z_derivs = []
        for i in range(3):
            self.x_derivs.append(self.x_spline.derivative(i + 1))
            self.z_derivs.append(self.z_spline.derivative(i + 1))

    def get_state(self, t):
        if self.type == "sinusoid":
            p = self.amplitude * np.sin(2.0 * np.pi / self.period * t +
                                        self.phase) + self.offset
            p_dot = self.amplitude * (2.0 * np.pi / self.period) * np.cos(
                2.0 * np.pi / self.period * t + self.phase)
            p_ddot = -self.amplitude * (2.0 * np.pi / self.period)**2 * np.sin(
                2.0 * np.pi / self.period * t + self.phase)
            p_dddot = -self.amplitude * (
                2.0 * np.pi / self.period
            )**3 * np.cos(2.0 * np.pi / self.period * t + self.phase)

            vx = p_dot.item(0)
            vy = p_dot.item(1)
            ax = p_ddot.item(0)
            ay = p_ddot.item(1)
            psi = np.arctan2(vy, vx)
            psi_dot = (vx * ay + vy * ax) / (vx**2 + vy**2)
            # psi = 10.*t
            # psi_dot = 10.0

            return DesiredState(p, p_dot, p_ddot, p_dddot, psi, psi_dot)

        elif self.type == "bspline":
            if t > self.knots[-1]:
                # p = np.array([[self.x_spline(self.knots[-1]).item(0), 0.0, self.z_spline(self.knots[-1]).item(0)]]).T
                p = np.array([[
                    0.0,
                    self.x_spline(self.knots[-1]).item(0),
                    self.z_spline(self.knots[-1]).item(0)
                ]]).T
                derivs = []
                for i in range(3):
                    derivs.append(np.zeros((3, 1)))
            else:
                # p = np.array([[self.x_spline(t).item(0), 0.0, self.z_spline(t).item(0)]]).T
                p = np.array(
                    [[0.0,
                      self.x_spline(t).item(0),
                      self.z_spline(t).item(0)]]).T
                derivs = []
                for i in range(3):
                    # derivs.append(np.array([[self.x_derivs[i](t).item(0), 0.0, self.z_derivs[i](t).item(0)]]).T)
                    derivs.append(
                        np.array([[
                            0.0, self.x_derivs[i](t).item(0),
                            self.z_derivs[i](t).item(0)
                        ]]).T)
            vx = derivs[0].item(0)
            vy = derivs[0].item(1)
            ax = derivs[1].item(0)
            ay = derivs[1].item(1)
            # psi = np.arctan2(vy, vx)
            # if (vx**2 + vy**2) == 0:
            #     psi_dot = 0.0
            # else:
            #     psi_dot = (vx*ay + vy*ax)/(vx**2 + vy**2)
            psi = 0.0
            psi_dot = 0.0

            return DesiredState(p, derivs[0], derivs[1], derivs[2], psi,
                                psi_dot)

    def get_plot_points(self):
        if self.type == "sinusoid":
            t = np.linspace(0.0, np.max(self.period), 1000)
            return self.amplitude * np.sin(2.0 * np.pi / self.period * t +
                                           self.phase) + self.offset

        elif self.type == "bspline":
            t = np.linspace(self.knots[0], self.knots[-1], 1000)
            plt.subplot(421)
            plt.title("x")
            plt.plot(t, self.x_spline(t))
            plt.subplot(422)
            plt.title("z")
            plt.plot(t, self.z_spline(t))
            plt.subplot(423)
            plt.title("x1")
            plt.plot(t, self.x_derivs[0](t))
            plt.subplot(424)
            plt.title("z1")
            plt.plot(t, self.z_derivs[0](t))
            plt.subplot(425)
            plt.title("x2")
            plt.plot(t, self.x_derivs[1](t))
            plt.subplot(426)
            plt.title("z2")
            plt.plot(t, self.z_derivs[1](t))
            plt.subplot(427)
            plt.title("x3")
            plt.plot(t, self.x_derivs[2](t))
            plt.subplot(428)
            plt.title("z3")
            plt.plot(t, self.z_derivs[2](t))
            plt.show()
            input()
            return np.vstack(
                [np.zeros(t.shape),
                 self.x_spline(t),
                 self.z_spline(t)])
Exemplo n.º 13
0
class bspline:
    def __init__(self, ctrl_pts):
        x = ctrl_pts[:, 0]
        y = ctrl_pts[:, 1]
        self.n_c = len(x)
        self.t = range(-ORDER, ORDER + self.n_c + 1)
        self.t_max = self.n_c
        self.t_min = 0
        self.c = np.array([x, y])
        self.update()

    def update(self):
        self.basis = []

        x = np.append(self.c[0], self.c[0, 0:ORDER])
        y = np.append(self.c[1], self.c[1, 0:ORDER])
        self.c_periodic = np.array([x, y])
        self.n_c_periodic = len(x)

        for i in range(self.n_c_periodic):
            self.basis.append(
                BSpline.basis_element(self.t[i:i + ORDER + 2], False))

        self.bsplx = BSpline(self.t, x, ORDER, False)
        self.bsply = BSpline(self.t, y, ORDER, False)
        self.derx1 = self.bsplx.derivative(1)
        self.dery1 = self.bsply.derivative(1)
        self.derx2 = self.bsplx.derivative(2)
        self.dery2 = self.bsply.derivative(2)

        # Sample some values
        self.sample_nb = NB_POINTS_BSPL
        self.sample_t = np.linspace(self.t_min,
                                    self.t_max,
                                    self.sample_nb,
                                    endpoint=True)
        self.sample_values = np.zeros((self.sample_nb, 2))
        for i in range(self.sample_nb):
            self.sample_values[i] = self.estimate(self.sample_t[i])

        #self.plot_curve(True)
        #plt.show()

    def estimate_with_basis(self, tk):
        tk = self.modulo_tk(tk)
        b = self.get_basis(tk)
        pt = np.array([0.0, 0.0])
        for i in range(self.n_c_periodic):
            pt[0] += b[i] * self.c_periodic[0][i]
            pt[1] += b[i] * self.c_periodic[1][i]
        return pt

    def get_basis(self, tk):
        tk = self.modulo_tk(tk)
        b_term = np.zeros(self.n_c_periodic)
        for i in range(self.n_c_periodic):
            tmp = self.basis[i](tk)
            if not np.isnan(tmp):
                b_term[i] = tmp
        return b_term

    def modulo_tk(self, tk):
        return (tk - self.t_min) % (self.t_max - self.t_min) + self.t_min

    def estimate(self, tk):
        tk = self.modulo_tk(tk)
        return np.array([self.bsplx(tk), self.bsply(tk)])

    def derivative1(self, tk):
        tk = self.modulo_tk(tk)
        return np.array([self.derx1(tk), self.dery1(tk)])

    def derivative2(self, tk):
        tk = self.modulo_tk(tk)
        return np.array([self.derx2(tk), self.dery2(tk)])

    def plot_curve(self, plot_ctrl_pts=False):
        plt.figure(utility.figMerge)
        u = np.linspace(self.t_min, self.t_max, self.sample_nb)
        pt = np.zeros((self.sample_nb, 2))
        #pt2 = np.zeros((self.sample_nb,2))
        for i in range(self.sample_nb):
            pt[i] = self.estimate(u[i])
            #pt2[i] = self.estimate_with_basis(u[i])
        plt.plot(pt[:, 0], pt[:, 1], 'b', linewidth=2)
        #plt.plot(pt2[:,0],pt2[:,1],'.r')
        if plot_ctrl_pts:
            plt.plot(np.append(self.c[0, :], self.c[0, 0]),
                     np.append(self.c[1, :], self.c[1, 0]),
                     'g',
                     linewidth=2)