#!/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.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] / j.scale + j.theta 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 = 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 = PumaFK((j1,j2,j3,j4,j5,j6)) if len(sys.argv) == 6+1: counts = [int(x) for x in sys.argv[1:]] r.update(counts) print [int(x) for x in r.p] 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] time.sleep(0.5)