In [1]:

    

from general_wheel import*
from mobile_ss import*


    

In [2]:

    

b1 = Symbol('\\beta _{1s}')
L = Symbol('L')
s1 = general_wheel(v_alpha = -pi/2, v_beta = b1, v_gamma =  0, d = 0, L= L)
s1.exp


    

    




New mobile robot parameterization






    
Out[2]:





$$\left[\begin{matrix}- r\\0\end{matrix}\right] \dot{\phi} + \left[\begin{matrix}\sin{\left (\beta _{{1s}} \right )} & - \cos{\left (\beta _{{1s}} \right )} & L \sin{\left (\beta _{{1s}} \right )}\\\cos{\left (\beta _{{1s}} \right )} & \sin{\left (\beta _{{1s}} \right )} & L \cos{\left (\beta _{{1s}} \right )}\end{matrix}\right] {R(\theta)} \dot{\xi}$$

In [3]:

    

b2 = Symbol('\\beta _{2s}')
s2 = general_wheel(v_alpha = +pi/2, v_beta = b2, v_gamma =  0, d = 0, L= L)
s2.exp


    

    




New mobile robot parameterization






    
Out[3]:





$$\left[\begin{matrix}- r\\0\end{matrix}\right] \dot{\phi} + \left[\begin{matrix}- \sin{\left (\beta _{{2s}} \right )} & \cos{\left (\beta _{{2s}} \right )} & L \sin{\left (\beta _{{2s}} \right )}\\- \cos{\left (\beta _{{2s}} \right )} & - \sin{\left (\beta _{{2s}} \right )} & L \cos{\left (\beta _{{2s}} \right )}\end{matrix}\right] {R(\theta)} \dot{\xi}$$

In [4]:

    

b3 = Symbol('\\beta _{3c}')
a = Symbol('a')
d = Symbol('d')
c3 = general_wheel(v_alpha = pi, v_beta = b3, v_gamma =  0, d = d, L= a)
c3.exp


    

    




New mobile robot parameterization






    
Out[4]:





$$\left[\begin{matrix}0\\d\end{matrix}\right] \dot{\beta} + \left[\begin{matrix}- r\\0\end{matrix}\right] \dot{\phi} + \left[\begin{matrix}- \cos{\left (\beta _{{3c}} \right )} & - \sin{\left (\beta _{{3c}} \right )} & a \sin{\left (\beta _{{3c}} \right )}\\\sin{\left (\beta _{{3c}} \right )} & - \cos{\left (\beta _{{3c}} \right )} & a \cos{\left (\beta _{{3c}} \right )} + d\end{matrix}\right] {R(\theta)} \dot{\xi}$$

In [5]:

    

mob = mobile_ss();   
# no denominator in Sigma[0,0]
mob.conf_kinematic_model([s1,s2,c3],['s','s','c'],simply=2)
#The last 4 rows can be symplified


    

    




New mobile robot instance:






    
Out[5]:





$$\left[\begin{matrix}2 L \sin{\left (\theta \right )} \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )} + L \sin{\left (\beta _{{1s}} + \beta _{{2s}} \right )} \cos{\left (\theta \right )} & 0 & 0\\L \sin{\left (\theta \right )} \sin{\left (\beta _{{1s}} + \beta _{{2s}} \right )} - 2 L \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )} \cos{\left (\theta \right )} & 0 & 0\\\sin{\left (\beta _{{1s}} - \beta _{{2s}} \right )} & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\\- \frac{L}{d} \sin{\left (\beta _{{3c}} \right )} \sin{\left (\beta _{{1s}} + \beta _{{2s}} \right )} - \frac{2 L}{d} \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )} \cos{\left (\beta _{{3c}} \right )} - \frac{1}{d} \left(a \cos{\left (\beta _{{3c}} \right )} + d\right) \sin{\left (\beta _{{1s}} - \beta _{{2s}} \right )} & 0 & 0\\\frac{L}{r} \sin{\left (\beta _{{1s}} \right )} \sin{\left (\beta _{{1s}} - \beta _{{2s}} \right )} + \frac{L}{r} \sin{\left (\beta _{{1s}} \right )} \sin{\left (\beta _{{1s}} + \beta _{{2s}} \right )} + \frac{2 L}{r} \cos^{2}{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )} & 0 & 0\\\frac{L}{r} \sin{\left (\beta _{{2s}} \right )} \sin{\left (\beta _{{1s}} - \beta _{{2s}} \right )} - \frac{L}{r} \sin{\left (\beta _{{2s}} \right )} \sin{\left (\beta _{{1s}} + \beta _{{2s}} \right )} - \frac{2 L}{r} \cos{\left (\beta _{{1s}} \right )} \cos^{2}{\left (\beta _{{2s}} \right )} & 0 & 0\\\frac{2 L}{r} \sin{\left (\beta _{{3c}} \right )} \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )} - \frac{L}{r} \sin{\left (\beta _{{1s}} + \beta _{{2s}} \right )} \cos{\left (\beta _{{3c}} \right )} + \frac{a}{r} \sin{\left (\beta _{{3c}} \right )} \sin{\left (\beta _{{1s}} - \beta _{{2s}} \right )} & 0 & 0\end{matrix}\right]$$

In [6]:

    

mob.C1_star


    

    
Out[6]:





$$\left[\begin{matrix}\cos{\left (\beta _{{1s}} \right )} & \sin{\left (\beta _{{1s}} \right )} & L \cos{\left (\beta _{{1s}} \right )}\\- \cos{\left (\beta _{{2s}} \right )} & - \sin{\left (\beta _{{2s}} \right )} & L \cos{\left (\beta _{{2s}} \right )}\end{matrix}\right]$$

In [7]:

    

mob.C1_star[0,:]*cos(b2)


    

    
Out[7]:





$$\left[\begin{matrix}\cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )} & \sin{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )} & L \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )}\end{matrix}\right]$$

In [8]:

    

mob.C1_star[1,:]*cos(b1)


    

    
Out[8]:





$$\left[\begin{matrix}- \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )} & - \sin{\left (\beta _{{2s}} \right )} \cos{\left (\beta _{{1s}} \right )} & L \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )}\end{matrix}\right]$$

In [9]:

    

c2 = simplify(mob.C1_star[0,:]*cos(b2) + mob.C1_star[1,:]*cos(b1))
c2


    

    
Out[9]:





$$\left[\begin{matrix}0 & \sin{\left (\beta _{{1s}} - \beta _{{2s}} \right )} & 2 L \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )}\end{matrix}\right]$$

In [10]:

    

C1_star = mob.C1_star[0,:].col_join(c2)
C1_star


    

    
Out[10]:





$$\left[\begin{matrix}\cos{\left (\beta _{{1s}} \right )} & \sin{\left (\beta _{{1s}} \right )} & L \cos{\left (\beta _{{1s}} \right )}\\0 & \sin{\left (\beta _{{1s}} - \beta _{{2s}} \right )} & 2 L \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )}\end{matrix}\right]$$

In [11]:

    

mob.Sigma


    

    
Out[11]:





$$\left[\begin{matrix}L \sin{\left (\beta _{{1s}} + \beta _{{2s}} \right )}\\- 2 L \cos{\left (\beta _{{1s}} \right )} \cos{\left (\beta _{{2s}} \right )}\\\sin{\left (\beta _{{1s}} - \beta _{{2s}} \right )}\end{matrix}\right]$$

In [12]:

    

mob.E


    

    
Out[12]:





$$\left[\begin{matrix}\frac{1}{r} \sin{\left (\beta _{{1s}} \right )} & - \frac{1}{r} \cos{\left (\beta _{{1s}} \right )} & \frac{L}{r} \sin{\left (\beta _{{1s}} \right )}\\- \frac{1}{r} \sin{\left (\beta _{{2s}} \right )} & \frac{1}{r} \cos{\left (\beta _{{2s}} \right )} & \frac{L}{r} \sin{\left (\beta _{{2s}} \right )}\\- \frac{1}{r} \cos{\left (\beta _{{3c}} \right )} & - \frac{1}{r} \sin{\left (\beta _{{3c}} \right )} & \frac{a}{r} \sin{\left (\beta _{{3c}} \right )}\end{matrix}\right]$$

In [13]:

    

mob.D


    

    
Out[13]:





$$\left[\begin{matrix}- \frac{1}{d} \sin{\left (\beta _{{3c}} \right )} & \frac{1}{d} \cos{\left (\beta _{{3c}} \right )} & - \frac{1}{d} \left(a \cos{\left (\beta _{{3c}} \right )} + d\right)\end{matrix}\right]$$

In [14]:

    

mob.C1_star


    

    
Out[14]:





$$\left[\begin{matrix}\cos{\left (\beta _{{1s}} \right )} & \sin{\left (\beta _{{1s}} \right )} & L \cos{\left (\beta _{{1s}} \right )}\\- \cos{\left (\beta _{{2s}} \right )} & - \sin{\left (\beta _{{2s}} \right )} & L \cos{\left (\beta _{{2s}} \right )}\end{matrix}\right]$$

In [21]:

    

sin(b1+pi/2)


    

    
Out[21]:





$$\cos{\left (\beta _{{1s}} \right )}$$