In [1]:

    

from general_wheel import*
from mobile_ss import*
from tools 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]:

    

b3 = Symbol('\\beta _{3c}')
a = Symbol('a')
c3 = general_wheel(v_alpha = pi, v_beta = b3 , v_gamma =  0, 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();   
S = mob.conf_kinematic_model([f1,f2,c3],['f','f','c'])
S


    

    




New mobile robot instance:






    
Out[5]:





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

In [6]:

    

mob.C1_star


    

    
Out[6]:





$$\left[\begin{matrix}0 & 1 & 0\\0 & 1 & 0\end{matrix}\right]$$

In [26]:

    

#q vector
p1 = Symbol('phi_1')
p2 = Symbol('phi_2')
p3 = Symbol('phi_3')
q  = [x,y,theta,b3,p1,p2,p3]
#involutive span of S
span = bracket_span([S[0:,0],S[0:,1]],q)


    

    




rank = 6 = to state vector

In [30]:

    

spanMatrix = list2matrix(span)


    

In [31]:

    

def is_reducible(span_z,vector_z):
    dimz,m = vector_z.shape
    if span_z.rank() < dimz:
        print "Reducible system, dim(span_z) < dim(z)"
        return True
    else:
        print "Non Reducible system dim(span_z) => dim(z)"
        return False


    

In [32]:

    

is_reducible(spanMatrix,Matrix(q))


    

    




Reducible system, dim(span_z) < dim(z)






    
Out[32]:





True