# puma / PumaIK.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 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178``` ```#!/usr/bin/python # Direct inverse kinesmatics for the PUMA. # # Based on "A geometric approach in solving the inverse kinematics of # PUMA robots" by Lee and Ziegler, 1983. # # http://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf?sequence=5 # # A is the approach vector of the tool # S is the sliding vector (the direction the tool would open) # N is the normal to those two import sys from math import pi, sin, cos, sqrt, atan2, fabs from Joint import Joint class PumaIK: def __init__(self, right, above, joints): if len(joints) != 6: raise Exception("PUMA must have six joints") self.joints = joints self.theta = [0,0,0,0,0,0] self.counts = [0,0,0,0,0,0] if right: self.right = 1 else: self.right = -1 if above: self.above = 1 else: self.above = -1 def ik(self, xyz, a, s, n): # Back project the position of the spherical ball joint # by the final dimension of the tool aligned along the # approach angle. j6 = self.joints[5] d6 = j6.d new_xyz = xyz new_xyz[0] -= d6 * a[0] new_xyz[1] -= d6 * a[1] new_xyz[2] -= d6 * a[2] print "new xyz: " + str(new_xyz) self.major(new_xyz) self.wrist(a, s, n) # Compute the major axis commands to position the spherical # joint at position XYZ, updating the first three elements of # theta[]. def major(self, xyz): px = xyz[0] py = xyz[1] pz = xyz[2] a2 = self.joints[1].a a3 = self.joints[2].a d2 = self.joints[1].d d4 = self.joints[3].d if px*px + py*py < d2*d2: print "Out of reach" return False r = sqrt(px*px + py*py - d2*d2) R = sqrt(px*px + py*py + pz*pz - d2*d2) # theta[0] defined in equation 26 self.theta[0] = atan2( -self.right * py * r - px * d2, -self.right * px * r + py * d2 ) # theta[1] is equations 28 - 35. sin_alpha = -pz / R cos_alpha = -self.right * r / R cos_beta = (a2*a2 + R*R - (d4*d4 + a3*a3)) / (2*a2*R) if cos_beta > 1 or cos_beta < -1: raise Exception("Beta out of range") sin_beta = sqrt(1 - cos_beta*cos_beta) sin_t2 = sin_alpha * cos_beta + self.right * self.above * cos_alpha * sin_beta cos_t2 = cos_alpha * cos_beta - self.right * self.above * sin_alpha * sin_beta self.theta[1] = atan2(sin_t2, cos_t2) # theta[2] t2 = d4*d4 + a3*a3 t = sqrt(t2) cos_phi = (a2*a2 + t2 - R*R) / (2 * a2 * t) if cos_phi > 1 or cos_phi < -1: raise Exception("Phi out of range") sin_phi = self.right * self.above * sqrt(1 - cos_phi*cos_phi) sin_beta = d4 / t cos_beta = fabs(a3) / t sin_t3 = sin_phi*cos_beta - cos_phi*sin_beta cos_t3 = cos_phi*cos_beta + sin_phi*sin_beta self.theta[2] = atan2(sin_t3, cos_t3) return True # Compute the wrist commands given the desired wrist configuration. # The major positions must have already been computed to position # the major axes. def wrist(self, a, s, n): M = 1 ax = a[0] ay = a[1] az = a[2] sx = s[0] sy = s[1] sz = s[2] nx = n[0] ny = n[1] nz = n[2] C1 = cos(self.theta[0]) S1 = sin(self.theta[0]) C23 = cos(self.theta[1] + self.theta[2]) S23 = sin(self.theta[1] + self.theta[2]) S4 = M * (C1*ay - S1*ax) C4 = M * (C1*C23*ax + S1*C23*ay- S23*az) self.theta[3] = atan2(S4, C4) S5 = (C1*C23*C4 - S1*S4)*ax + (S1*C23*C4 + C1*S4)*ay - C4* S23*az C5 = C1*S23*ax + S1*S23*ay + C23*az self.theta[4] = atan2(S5,C5) S6 = (-S1*C4 - C1*C23*S4)*nx + (C1*C4-S1*C23*S4)*ny + S4*S23*nz C6 = (-S1*C4-C1*C23*S4)*sx + (C1*C4-S1*C23*S4)*sy + S4*S23*sz self.theta[5] = atan2(S6,C6) return True def get_counts(self): counts = [] for i in range(0, 6): j = self.joints[i] a = self.theta[i] - j.theta if a > pi: a -= 2 * pi elif a < -pi: a += 2 * pi c = a * j.scale if j.coupling: c += counts[i-1] * j.coupling counts.append(int(c)) return counts if __name__ == "__main__": j1 = Joint(0, 0, 0, -11200 - +12000, 0) j2 = Joint(130, 205, -90, +18000 - -18000, 0) j3 = Joint(0, 0, +90, -10500 - +11200, 0) j4 = Joint(225, 0, -90, +17400, 0) j5 = Joint(0, 0, 0, -8000 - +8000, -6917/30674.0) j6 = Joint(50, 0, 0, +6500 - -6500, 1200/-10000.0) r = PumaIK(True, True, (j1,j2,j3,j4,j5,j6)) if len(sys.argv) != 3+1: sys.exit(0) pos = [int(x) for x in sys.argv[1:]] a = [0, 1, 0] s = [1, 0, 0] n = [0, 0, 1] r.ik(pos, a, s, n) print " ".join([str(int(x*180/pi)) for x in r.theta]) print " ".join([str(x) for x in r.get_counts()]) print "go,10000," + ",".join([str(x) for x in r.get_counts()]) ```
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.