Untitled

import numpy as np
from math import pi,cos,sin

th = [4,1,2,0,0,0]
a = [0.01,0.28,0.068,0,0,0]
d = [0.179,0,0,0.271,0,0.0766]
alpha = [pi/2,0,pi/2,-pi/2,pi/2,0]

joints = []

for i in range(6):
    joints.append(np.matrix([[cos(th[i]),-sin(th[i])*cos(alpha[i]), sin(th[i])*sin(alpha[i]), a[i]*cos(th[i])],
            [sin(th[i]),cos(th[i])*cos(alpha[i]), -cos(th[i])*sin(alpha[i]), a[i]*sin(th[i])],
            [0,sin(alpha[i]),cos(alpha[i]),d[i]],
            [0,0,0,1]]))

print(joints[5]*joints[4]*joints[3]*joints[2]*joints[1]*joints[0])

print(d[1]+a[2]+a[3]," :x")
print(d[0]+a[1]+a[2]," :y")
print(a[0]+d[3]+d[5]," :z")