#!/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 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 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: print "Beta out of range" return False 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: print "Phi out of range" return False 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] c = (self.theta[i] - j.theta) * j.scale / pi 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, +8000 - -8000, 0) j5 = Joint(0, 0, 0, -8000 - +8000, 9000/-40000.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:]] r.major(pos) print r.get_counts()