Kinematics

In [1]:

    

import sympy
from sympy import *
sympy.init_printing()


    

In [2]:

    

def Rx(t):
    return Matrix([ [1, 0, 0],
                    [0, cos(t), -sin(t)],
                    [0, sin(t), cos(t)]])
                    
def Ry(t):
    return Matrix([ [cos(t), 0, sin(t)],
                    [0, 1, 0],
                    [-sin(t), 0, cos(t)]])
                    
def Rz(t):
    return Matrix([ [cos(t), -sin(t), 0],
                    [sin(t), cos(t), 0],
                    [0, 0, 1]])


    

In [3]:

    

theta1 = Symbol('theta1')
theta2 = Symbol('theta2')
theta3 = Symbol('theta3')


    

In [4]:

    

x1, y1, z1 = symbols('x1 y1 z1')
x2, y2, z2 = symbols('x2 y2 z2')
x3, y3, z3 = symbols('x3 y3 z3')


    

In [5]:

    

l1, l2 = symbols('l1 l2')


    

In [6]:

    

Rx(theta2).dot(Matrix([[0], [0], [-l1]]))


    

    
Out[6]:





$$\left [ 0, \quad l_{1} \sin{\left (\theta_{2} \right )}, \quad - l_{1} \cos{\left (\theta_{2} \right )}\right ]$$

In [7]:

    

temp = Rx(theta2).dot(Matrix([[0], [0], [-l1]]))
Ry(theta1).dot(temp)


    

    
Out[7]:





$$\left [ - l_{1} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )}, \quad l_{1} \sin{\left (\theta_{2} \right )}, \quad - l_{1} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )}\right ]$$

In [8]:

    

Rx(theta2).dot(Ry(theta1))


    

    
Out[8]:





$$\left [ \cos{\left (\theta_{1} \right )}, \quad 0, \quad - \sin{\left (\theta_{1} \right )}, \quad \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )}, \quad \cos{\left (\theta_{2} \right )}, \quad \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )}, \quad \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )}, \quad - \sin{\left (\theta_{2} \right )}, \quad \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )}\right ]$$

In [25]:

    

p3 = Matrix([[-l2 * sin(theta3)], [0], [-l1 -l2 * cos(theta3)]])


    

In [33]:

    

a = Rx(theta2).dot(p3)
b = Ry(theta1).dot(Matrix(a))
Matrix(b)


    

    
Out[33]:





$$\left[\begin{matrix}- l_{2} \sin{\left (\theta_{3} \right )} \cos{\left (\theta_{1} \right )} + \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )}\\- \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )}\\l_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{3} \right )} + \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )}\end{matrix}\right]$$

In [ ]:

    

Rx(theta2).dot(p3)


    

In [11]:

    

Rx(theta2).dot(p3)


    

    
Out[11]:





$$\left [ - l_{2} \sin{\left (\theta_{3} \right )}, \quad - \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )}, \quad \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \cos{\left (\theta_{2} \right )}\right ]$$

In [12]:

    

x1, y1, z1 = Rx(theta2).dot(p3)


    

In [13]:

    

x1


    

    
Out[13]:





$$- l_{2} \sin{\left (\theta_{3} \right )}$$

In [14]:

    

y1


    

    
Out[14]:





$$- \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )}$$

In [15]:

    

z1


    

    
Out[15]:





$$\left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \cos{\left (\theta_{2} \right )}$$

In [16]:

    

l1, l2, l3 = symbols('l1 l2 l3')


    

In [17]:

    

x1 = l1 * cos(theta1)


    

In [18]:

    

z1 = -l1 * sin(theta1)


    

In [19]:

    

y1 = 0


    

In [20]:

    

a = Rz(theta2).dot(p3)
b = Ry(theta1).dot(a)
b


    

    
Out[20]:





$$\left [ - l_{2} \sin{\left (\theta_{3} \right )} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} + \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \sin{\left (\theta_{1} \right )}, \quad - l_{2} \sin{\left (\theta_{2} \right )} \sin{\left (\theta_{3} \right )}, \quad l_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{3} \right )} \cos{\left (\theta_{2} \right )} + \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \cos{\left (\theta_{1} \right )}\right ]$$

In [34]:

    

a = Rz(theta2).dot(p3)
b = Ry(theta1).dot(Matrix(a))
Matrix(b)


    

    
Out[34]:





$$\left[\begin{matrix}- l_{2} \sin{\left (\theta_{3} \right )} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} + \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \sin{\left (\theta_{1} \right )}\\- l_{2} \sin{\left (\theta_{2} \right )} \sin{\left (\theta_{3} \right )}\\l_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{3} \right )} \cos{\left (\theta_{2} \right )} + \left(- l_{1} - l_{2} \cos{\left (\theta_{3} \right )}\right) \cos{\left (\theta_{1} \right )}\end{matrix}\right]$$

In [22]:

    

p1 = Matrix([[0], [0], [-l1]])


    

In [35]:

    

a = Rz(theta2).dot(p1)
b = Ry(theta1).dot(Matrix(a))
Matrix(b)


    

    
Out[35]:





$$\left[\begin{matrix}- l_{1} \sin{\left (\theta_{1} \right )}\\0\\- l_{1} \cos{\left (\theta_{1} \right )}\end{matrix}\right]$$

In [ ]: