trajectory.py 文件源码

def __init__(self, states, interval, method, differential_order=0):
        """
        :param states: tuple of states in beginning and end of interval
        :param interval: time interval (tuple)
        :param method: method to use (``poly`` or ``tanh``)
        :param differential_order: grade of differential flatness :math:`\\gamma`
        """
        self.yd = states
        self.t0 = interval[0]
        self.t1 = interval[1]
        self.dt = interval[1] - interval[0]

        # setup symbolic expressions
        if method == "tanh":
            tau, sigma = sp.symbols('tau, sigma')
            # use a gevrey-order of alpha = 1 + 1/sigma
            sigma = 1.1
            phi = .5*(1 + sp.tanh(2*(2*tau - 1)/((4*tau*(1-tau))**sigma)))

        elif method == "poly":
            gamma = differential_order  # + 1 # TODO check this against notes
            tau, k = sp.symbols('tau, k')

            alpha = sp.factorial(2 * gamma + 1)

            f = sp.binomial(gamma, k) * (-1) ** k * tau ** (gamma + k + 1) / (gamma + k + 1)
            phi = alpha / sp.factorial(gamma) ** 2 * sp.summation(f, (k, 0, gamma))
        else:
            raise NotImplementedError("method {} not implemented!".format(method))

        # differentiate phi(tau), index in list corresponds to order
        dphi_sym = [phi]  # init with phi(tau)
        for order in range(differential_order):
            dphi_sym.append(dphi_sym[-1].diff(tau))

        # lambdify
        self.dphi_num = []
        for der in dphi_sym:
            self.dphi_num.append(sp.lambdify(tau, der, 'numpy'))