In [1]:

    

from general_wheel import*
from mobile_ss import*


    

In [2]:

    

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


    

    




New mobile robot parameterization






    
Out[2]:





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

In [3]:

    

b2 = Symbol('\\beta _{2c}')
d = Symbol('d')
c2 = general_wheel(v_alpha = pi/2, v_beta = b2, v_gamma =  0, d = d, L= L*2**.5)
c2.exp


    

    




New mobile robot parameterization






    
Out[3]:





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

In [4]:

    

b3 = Symbol('\\beta _{3c}')
d = Symbol('d')
c3 = general_wheel(v_alpha = pi, v_beta = b3, v_gamma =  0, d = d, L= L*2**.5)
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 )} & 1.4142135623731 L \sin{\left (\beta _{{3c}} \right )}\\\sin{\left (\beta _{{3c}} \right )} & - \cos{\left (\beta _{{3c}} \right )} & 1.4142135623731 L \cos{\left (\beta _{{3c}} \right )} + d\end{matrix}\right] {R(\theta)} \dot{\xi}$$

In [5]:

    

mob = mobile_ss();  
#No singularity with 2
simplify(mob.conf_kinematic_model([s1,c2,c3],['s','c','c'],simply=2))


    

    




New mobile robot instance:






    
Out[5]:





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

In [6]:

    

from sympy import *
mob.C1_star.nullspace()


    

    
Out[6]:





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

In [7]:

    

mob.J1


    

    
Out[7]:





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

In [8]:

    

mob.J2


    

    
Out[8]:





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

In [9]:

    

mob.C1


    

    
Out[9]:





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

In [10]:

    

mob.Sigma


    

    
Out[10]:





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

In [ ]: