# puma / PumaFK.py

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125``` ```#!/usr/bin/python # Compute the XYZ and tool orientation for a Puma given the # six joint counters and a DH configuration table. from math import pi, cos, sin import sys import time import MDC from Joint import Joint class PumaFK(object): def __init__(self, joints): if len(joints) != 6: return False self.joints = joints self.n = [0,0,0] self.s = [0,0,0] self.a = [0,0,0] self.p = [0,0,0] self.theta = [0,0,0,0,0,0] def update(self, counts): if len(counts) != 6: return False; for i in range(0,6): j = self.joints[i] count = counts[i] if j.coupling: count -= counts[i-1] * j.coupling angle = count / j.scale + j.theta self.theta[i] = angle c1 = cos(self.theta[0]) c2 = cos(self.theta[1]) c3 = cos(self.theta[2]) c4 = cos(self.theta[3]) c5 = cos(self.theta[4]) c6 = cos(self.theta[5]) s1 = sin(self.theta[0]) s2 = sin(self.theta[1]) s3 = sin(self.theta[2]) s4 = sin(self.theta[3]) s5 = sin(self.theta[4]) s6 = sin(self.theta[5]) c23 = cos(self.theta[1] + self.theta[2]) s23 = sin(self.theta[1] + self.theta[2]) tn1 = c23*(c4*c5*c6 - s4*s6) - s23*s5*c6 tn2 = s4*c5*c6 + c4*s6 # equation 5 self.n = [ c1*tn1 - s1*tn2, s1*tn1 + c1*tn2, -s23*(c4*c5*c6 - s4*s6) - c23*s5*c6 ] ts1 = -c23*(c4*c5*c6 + s4*c6) + s23*s5*s6 ts2 = -s4*c5*s6 + c4*c6 # equation 6 self.s = [ c1*ts1 - s1*ts2, s1*ts1 + c1*ts2, s23*(c4*c5*s6 + s4*c6) + c23*s5*s6 ] # equation 7 ta1 = c23*c4*s5 + s23*c5 ta2 = s4*s5 self.a = [ c1*ta1 - s1*ta2, s1*ta1 + c1*ta2, -s23*c4*s5 + c23*c5 ] a2 = self.joints[1].a d2 = self.joints[1].d a3 = self.joints[2].a d4 = self.joints[3].d d6 = self.joints[5].d tp1 = d6*(c23*c4*s5 + s23*c5) + s23*d4 + a3*c23 + a2*c2 tp2 = d6*s4*s5 + d2 self.p = [ c1*tp1 - s1*tp2, s1*tp1 + c1*tp2, d6*(c23*c5 - s23*c4*s5) + c23*d4 - a3*s23 - a2*s2 ] if __name__ == "__main__": import PumaConfig r = PumaFK(PumaConfig.joints) if len(sys.argv) == 6+1: counts = [int(x) for x in sys.argv[1:]] r.update(counts) print [int(x*180/pi) for x in r.theta] print [int(x) for x in r.p] print [round(x,2) for x in r.n] print [round(x,2) for x in r.s] print [round(x,2) for x in r.a] sys.exit(0) sys.exit(-1) devices = ["/dev/tty.usbmodemfd1231", "/dev/tty.usbmodemfd1241", "/dev/tty.usbmodemfd1271"] controllers = [MDC.MDC(dev) for dev in devices] time.sleep(2) while True: counts = MDC.get_counts() r.update([int(x) for x in counts]) #print [int(x) for x in counts], [int(x) for x in r.p], [round(x,3) for x in r.s], [round(x,3) for x in r.n] print [int(x) for x in r.p], [round(x,3) for x in r.a], [round(x,3) for x in r.s], [round(x,3) for x in r.n] #print [int(x) for x in counts], [int(x*180/pi) for x in r.theta] time.sleep(0.5) ```