# puma / PumaFK.py

 ```Trammell Hudson 84af5a0 2013-07-07 ``` ``` 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``` ```#!/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 class PumaJoint(object): def __init__(self, d, a, theta, scale, coupling): if scale == 0: raise Exception("Invalid scale") self.d = d self.a = a self.theta = theta * pi / 180 self.scale = scale self.coupling = coupling 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.angles = [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 = counts[i] * pi / j.scale self.angles[i] = angle c1 = cos(self.angles[0]) c2 = cos(self.angles[1]) c3 = cos(self.angles[2]) c4 = cos(self.angles[3]) c5 = cos(self.angles[4]) c6 = cos(self.angles[5]) s1 = sin(self.angles[0]) s2 = sin(self.angles[1]) s3 = sin(self.angles[2]) s4 = sin(self.angles[3]) s5 = sin(self.angles[4]) s6 = sin(self.angles[5]) c23 = cos(self.angles[1] + self.angles[2]) s23 = sin(self.angles[1] + self.angles[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__": j1 = PumaJoint(0, 0, 0, -11200 - +12000, 0) j2 = PumaJoint(130, 205, -90, +18000 - -18000, 0) j3 = PumaJoint(0, 0, +90, -10500 - +11200, 0) j4 = PumaJoint(225, 0, 0, +8000 - -8000, 0) j5 = PumaJoint(0, 0, 0, -8000 - +8000, 9000/-40000.0) j6 = PumaJoint(0, 0, 0, -6500 - +6500, 1200/-10000.0) r = PumaFK((j1,j2,j3,j4,j5,j6)) counts = [int(x) for x in sys.argv[1:]] r.update(counts) print r.p ```
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.