# 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 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202``` ```#!/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 = list(xyz) new_xyz[0] -= d6 * a[0] new_xyz[1] -= d6 * a[1] new_xyz[2] -= d6 * a[2] print "new xyz: " + ",".join([str(int(x)) for x in 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) print "S4=" + str(S4) + " sin(4)=" + str(sin(self.theta[3])) print "C4=" + str(C4) + " cos(4)=" + str(cos(self.theta[3])) # Replace S4/C4 with the computed values based on theta4 # Degenerate cases might have a bad value in C4 if S4 is zero S4 = sin(self.theta[3]) C4 = cos(self.theta[3]) 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, current_theta): return self.angle2count(self.theta, current_theta) def angle2count(self, dest_theta, current_theta): counts = [] for i in range(0, 6): # Adjust by the home position offset to the # joint-frame angle. j = self.joints[i] a = dest_theta[i] # Elbow joint is off by 90 deg if i == 2 and a < 0: a += 2 * pi # try adjusting to avoid a wrap around # on the wrist joints that can do more than # 360 degrees. if False and (i == 3 or i == 5): if a - current_theta[i] > 1.5*pi: a -= 2 * pi elif a - current_theta[i] < -1.5*pi: a += 2 * pi # Adjust based on the joint configuration a -= j.theta # Convert to a count c = a * j.scale if j.coupling: c += counts[i-1] * j.coupling counts.append(int(c)) return counts if __name__ == "__main__": import PumaConfig r = PumaIK(True, True, PumaConfig.joints) 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] a = [0.03, 0.99, -0.13] s = [-0.93, -0.02, -0.38] n = [0.38, -0.13, -0.91] 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()]) ```