In [1]:

    

from general_wheel import*
from mobile_ss import*


    

In [2]:

    

f1 = general_wheel(v_alpha = -pi/2, v_beta = 0, v_gamma =  pi/2, d = 0)
f1.exp


    

    




New mobile robot parameterization






    
Out[2]:





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

In [3]:

    

f2 = general_wheel(v_alpha = pi/2, v_beta = 0, v_gamma =  -pi/2, d = 0)
f2.exp


    

    




New mobile robot parameterization






    
Out[3]:





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

In [4]:

    

a = Symbol('a')
b3s = Symbol('\\beta _{3s}')

s3 = general_wheel(v_alpha = 0, v_beta = b3s, v_gamma =  0, d = 0, L=a)
s3.exp


    

    




New mobile robot parameterization






    
Out[4]:





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

In [5]:

    

mob = mobile_ss();   
J1,J2,C1,C2,C1c,C2c,C1_star,w_types = mob.conf_kinematic_model([f1,f2,s3],['f','f','s'])
C1_star


    

    




New mobile robot instance:






    
Out[5]:





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

In [14]:

    

m0 = Matrix(([cos(theta), sin(theta), 0], [-sin(theta), cos(theta), 0], [0,0,1]))
R0m = Matrix(([cos(theta), -sin(theta), 0], [sin(theta), cos(theta), 0], [0,0,1]))

S = []
#This gives as result a list o vectors
Sigma = C1_star.nullspace()
#Covert to matriz
mn = len(Sigma)
for i in range(mn):
    Sigma[i] = Sigma[i].T
Sigma = Matrix(Sigma).T
Sigma


    

    
Out[14]:





$$\left[\begin{matrix}\frac{a \cos{\left (\beta _{{3s}} \right )}}{\sin{\left (\beta _{{3s}} \right )}}\\0\\1\end{matrix}\right]$$

In [15]:

    

# xi_dot 
xi_dot = R0m*Sigma
xi_dot

S.append(xi_dot)


    

In [16]:

    

#Bs_dot
ns = 0
for w in w_types:
    if(w == 's'):
        ns = ns + 1
if(ns > 0):
    Bs_dot = zeros(3,ns)
    S.append(Bs_dot)


    

In [17]:

    

#Bc_dot
if(len(C2c) != 0):
    D = -(C2c**-1)*C1c
    Bc_dot = D*Sigma
    S.append(Bc_dot)


    

In [18]:

    

#psi_dot
E = -J2**-1*J1
psi_dot = E*Sigma
S.append(psi_dot)
psi_dot


    

    
Out[18]:





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

In [19]:

    

#F (motorization)
if(len(C2c) != 0):
    F = Bc_dot.col_join(psi_dot)
else:
    F = psi_dot
F


    

    
Out[19]:





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

In [23]:

    

# S (Configurator kinematic model)
S = Matrix(S)
S = S.row_join(S2)


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-23-a5a9bb7c9427> in <module>()
      1 # S (Configurator kinematic model)
      2 S = Matrix(S)
----> 3 S = S.row_join(S2)

NameError: name 'S2' is not defined

In [ ]: