puma / PumaFK.py

#!/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)
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.