1. Trammell Hudson
  2. puma

Source

puma / PumaFK.py

Trammell Hudson 84af5a0 





Trammell Hudson 6024e4b 

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




















Trammell Hudson 0f6ce87 
Trammell Hudson 84af5a0 


































































Trammell Hudson 2d73c51 





Trammell Hudson 84af5a0 

Trammell Hudson 2d73c51 
Trammell Hudson 6024e4b 

Trammell Hudson 0f6ce87 
Trammell Hudson 6024e4b 
Trammell Hudson 84af5a0 
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.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)