Source

puma / PumaFK.py

Trammell Hudson 84af5a0 





Trammell Hudson 6024e4b 

Trammell Hudson 84af5a0 
Trammell Hudson 2d73c51 
Trammell Hudson 84af5a0 









Trammell Hudson 99ab2fa 
Trammell Hudson 84af5a0 









Trammell Hudson aaef81b 
Trammell Hudson 99ab2fa 
Trammell Hudson 84af5a0 
Trammell Hudson 99ab2fa 





Trammell Hudson 84af5a0 
Trammell Hudson 99ab2fa 





Trammell Hudson 84af5a0 
Trammell Hudson 99ab2fa 

Trammell Hudson 84af5a0 






Trammell Hudson 2177320 
Trammell Hudson 84af5a0 








































Trammell Hudson 0385ad0 

Trammell Hudson 84af5a0 
Trammell Hudson 2d73c51 
Trammell Hudson 6024e4b 

Trammell Hudson ba411c3 
Trammell Hudson 0f6ce87 
Trammell Hudson ba411c3 


Trammell Hudson 6024e4b 
Trammell Hudson 84af5a0 
Trammell Hudson ba411c3 
Trammell Hudson 6024e4b 







Trammell Hudson aef2c9d 

Trammell Hudson 2177320 
Trammell Hudson 6024e4b 
#!/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.theta = [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 = count / j.scale + j.theta
			self.theta[i] = angle

		c1 = cos(self.theta[0])
		c2 = cos(self.theta[1])
		c3 = cos(self.theta[2])
		c4 = cos(self.theta[3])
		c5 = cos(self.theta[4])
		c6 = cos(self.theta[5])

		s1 = sin(self.theta[0])
		s2 = sin(self.theta[1])
		s3 = sin(self.theta[2])
		s4 = sin(self.theta[3])
		s5 = sin(self.theta[4])
		s6 = sin(self.theta[5])

		c23 = cos(self.theta[1] + self.theta[2])
		s23 = sin(self.theta[1] + self.theta[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__":
	import PumaConfig
	r = PumaFK(PumaConfig.joints)

	if len(sys.argv) == 6+1:
		counts = [int(x) for x in sys.argv[1:]]
		r.update(counts)
		print [int(x*180/pi) for x in r.theta]
		print [int(x) for x in r.p]
		print [round(x,2) for x in r.n]
		print [round(x,2) for x in r.s]
		print [round(x,2) for x in r.a]
		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], [round(x,3) for x in r.s], [round(x,3) for x in r.n]
		print [int(x) for x in r.p], [round(x,3) for x in r.a], [round(x,3) for x in r.s], [round(x,3) for x in r.n]
		#print [int(x) for x in counts], [int(x*180/pi) for x in r.theta]
		time.sleep(0.5)