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]:

    

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


    

In [3]:

    

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

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


    

In [4]:

    

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


    

    
Out[4]:





$$\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 [5]:

    

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


    

    
Out[5]:





$$\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 [6]:

    

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


    

In [7]:

    

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 [8]:

    

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


    

In [9]:

    

M = sub_derlist(M, l)
M


    

    
Out[9]:





$$\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 [10]:

    

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


    

In [11]:

    

factor(M, V)


    

    
Out[11]:





$$\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 \sin{\left (\theta{\left (t \right )} \right )} + d \theta^{{(1)}}{\left (t \right )} \cos{\left (\gamma + \theta{\left (t \right )} \right )}\end{matrix}\right]$$

In [ ]: