Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- def forwardK(q):
- #T06 is the location of Wrist frame, {W}, relative to Base frame, {B}
- T01 = genT(q[0],0,d[0],0)
- T12 = genT(q[1],a[0],d[1],alpha[0])
- T23 = genT(q[2],a[1],d[2],alpha[1])
- T34 = genT(q[3],a[2],d[3],alpha[2])
- T45 = genT(q[4],a[3],d[4],alpha[3])
- T56 = genT(q[5],a[4],d[5],alpha[4])
- #Tool frame {T}
- #T67 = genT(0,0,d[5],0)
- T03 = matmul(T01,T12,T23)
- T36 = matmul(T34,T45,T56)
- T06 = matmul(T03,T36)
- #T07 = matmul(T06,T67)
- x = T[0][3]
- y = T[1][3]
- z = T[2][3]
- print("X: ",x)
- print("Y: ",y)
- print("Z: ",z,"n")
- print("T: ",T,"n")
- return T06
- def genT(theta, a, d, alpha):
- T = array([[cos(theta), (-sin(theta)), 0, a],
- [sin(theta)*cos(alpha), (cos(theta)*cos(alpha)), -sin(alpha), (- d*sin(alpha))],
- [sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d],
- [0, 0, 0, 1]])
- return T
- T = forwardK([30,-110,-30,0,0,0])
- x = T[0][3]
- y = T[1][3]
- z = T[2][3]
- R = T[0:3,0:3]
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement