In [1]:

    

from sympy.abc import*
from sympy import *
from sympy import MatrixSymbol, Identity
init_printing(use_latex=True) #For good look equations
from tools import*


    

In [2]:

    

t = Symbol('t')

def def_states(string = 'x', num_states = 5,  time_dep = True):
    """Define a list of states as sympy Functions or Symbols
    :param string: state symbol in a string format
    :param num_states: max number of states
    :param time_dep: if True the states are time depend
    :returns: list of states (Functions or Symbols)
    :Example:
    
    from sympy.abc import*
    from sympy import *
    x1,x2,x3,x4,x5 = def_states('x',5,True)
    """
    new_def = []
    
    for i in range(1,num_states+1):
        string = string +"_" +str(i)
        if time_dep:
            var = Function(string)(t)
        else:
            var = Symbol(string)
        new_def.append(var)
    return new_def


def def_state_der (string = 'y', num_der = 4,  time_dep = True):
    """Define a list of states derivatives as sympy Functions or Symbol

    :param string: state symbol in a string format
    :param num_der: maximum degree
    :param time_dep: if True the states are time depend
    :returns: list of states (Functions or Symbols)
    """
    new_def = []
    for i in range(1,num_der+1):
        nstring =  string + "^{(" + str(i) + ")}"
        if time_dep:
            var = Function(nstring)(t)
        else:
            var = Symbol(nstring)
        new_def.append(var)
    return new_def

def def_vars(varlist, time_dep = True):
    #Define variables respect to time
    """Define a variable list as sympy Functions or Symbol

    :param varlist: list of strings
    :param time_dep: if True the states are time depend
    :returns: list of variables (Functions or Symbols)
    """
    new_def = []
    for string in varlist:
        if time_dep:
            var = Function(string)(t)
        else:
            var = Symbol(string)
        new_def.append(var) 
    return new_def


    

In [3]:

    

#Functions respect to time (Variables)
x,y,th = def_vars (['x','y','\\theta'],True)


    

In [14]:

    

#Constants
b3s = Function('\\beta _{3s}')(t)

#Constants
d = Symbol('d')
gamma = Symbol('\\gamma')


    

In [15]:

    

z1 = Matrix([[x + d*cos(th + gamma)],  [ y + d*sin(th + gamma)]])
z1


    

    
Out[15]:





$$\left[\begin{matrix}d \cos{\left (\gamma + \theta{\left (t \right )} \right )} + x{\left (t \right )}\\d \sin{\left (\gamma + \theta{\left (t \right )} \right )} + y{\left (t \right )}\end{matrix}\right]$$

In [16]:

    

M = simplify(z1.diff(t))
M


    

    
Out[16]:





$$\left[\begin{matrix}- d \sin{\left (\gamma + \theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} + \frac{d}{d t} x{\left (t \right )}\\d \cos{\left (\gamma + \theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} + \frac{d}{d t} y{\left (t \right )}\end{matrix}\right]$$

In [17]:

    

x1,x2,x3,x4,x5 = def_states('x',5,True)


    

In [24]:

    

thd, thdd = def_state_der('\\theta',2,True)
xd, xdd = def_state_der('x',2,True)
yd, ydd = def_state_der('y',2,True)


    

In [19]:

    

def sub_der (M, var, derivatives):
    #Subtitute variable by their derivatives
    temp =  var
    for i in derivatives:
        temp = Derivative(temp)
        M = M.subs(temp,i)
    return M


    

In [20]:

    

l = ['x','y','\\theta']

def sub_derlist(M,list_var):
    #Subtitute a list of variable by their derivatives
    for var in list_var:
        M  = sub_der (M, Function(var)(t), def_state_der(var,5,True))
    return M


    

In [21]:

    

M = sub_derlist(M, l)
M


    

    
Out[21]:





$$\left[\begin{matrix}- d \theta^{{(1)}}{\left (t \right )} \sin{\left (\gamma + \theta{\left (t \right )} \right )} + \operatorname{x^{{(1)}}}{\left (t \right )}\\d \theta^{{(1)}}{\left (t \right )} \cos{\left (\gamma + \theta{\left (t \right )} \right )} + \operatorname{y^{{(1)}}}{\left (t \right )}\end{matrix}\right]$$

In [25]:

    

V = Symbol('V')
M = M.subs(xd,V*cos(th))
M = M.subs(yd,V*sin(th))


    

In [26]:

    

M


    

    
Out[26]:





$$\left[\begin{matrix}V \cos{\left (\theta{\left (t \right )} \right )} - d \theta^{{(1)}}{\left (t \right )} \sin{\left (\gamma + \theta{\left (t \right )} \right )}\\V \cos{\left (\theta{\left (t \right )} \right )} + d \theta^{{(1)}}{\left (t \right )} \cos{\left (\gamma + \theta{\left (t \right )} \right )}\end{matrix}\right]$$

In [ ]: