Kinematics



In [24]:

    
import sympy
from sympy import *
sympy.init_printing()



In [25]:

    
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 [26]:

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



In [27]:

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



In [28]:

    
l1, l2 = symbols('l1 l2')



In [29]:

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









    Out[29]:





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



In [30]:

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









    Out[30]:





$$\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 [31]:

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









    Out[31]:





$$\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 [32]:

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



In [33]:

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









    



---------------------------------------------------------------------------
AttributeError                            Traceback (most recent call last)
<ipython-input-33-2d672be4b4cf> in <module>()
----> 1 Rx(theta2).dot(Ry(theta1)).dot(p3)

AttributeError: 'list' object has no attribute 'dot'



In [63]:

    
a = Ry(theta1).dot(Rx(theta2))
a = Matrix(a)
a.rows = 3 
a.cols = 3
Matrix(a.dot(p3))









    Out[63]:





$$\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 [35]:

    
Rx(theta2).dot(p3)









    Out[35]:





$$\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 [36]:

    
Rx(theta2).dot(p3)









    Out[36]:





$$\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 [37]:

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



In [38]:

    
x1









    Out[38]:





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



In [39]:

    
y1









    Out[39]:





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



In [40]:

    
z1









    Out[40]:





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

I'm going to try to fix other people's problems.



In [41]:

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



In [42]:

    
x1 = l1 * cos(theta1)



In [43]:

    
z1 = -l1 * sin(theta1)



In [44]:

    
y1 = 0



In [45]:

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









    Out[45]:





$$\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 [46]:

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









    Out[46]:





$$\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 )} \cos{\left (\theta_{2} \right )}\\l_{2} \sin{\left (\theta_{2} \right )} \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 )} \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 )}\end{matrix}\right]$$



In [49]:

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



In [50]:

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









    Out[50]:





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



In [ ]: