Untitled

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

th = [0,0+pi/2,0,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]
            ]))

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

print(prod[[0],[3]]," :x")
print(prod[[1],[3]]," :y")
print(prod[[2],[3]]," :z")