puma / PumaFK.py

Trammell Hudson 84af5a0 



















































































































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

class PumaJoint(object):
	def __init__(self, d, a, theta, scale, coupling):
		if scale == 0:
			raise Exception("Invalid scale")
		self.d = d
		self.a = a
		self.theta = theta * pi / 180
		self.scale = scale
		self.coupling = coupling

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] * pi / j.scale
			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 = PumaJoint(0, 0, 0, -11200 - +12000, 0)
	j2 = PumaJoint(130, 205, -90, +18000 - -18000, 0)
	j3 = PumaJoint(0, 0, +90, -10500 - +11200, 0)
	j4 = PumaJoint(225, 0, 0, +8000 - -8000, 0)
	j5 = PumaJoint(0, 0, 0, -8000 - +8000, 9000/-40000.0)
	j6 = PumaJoint(0, 0, 0, -6500 - +6500, 1200/-10000.0)
	r = PumaFK((j1,j2,j3,j4,j5,j6))

	counts = [int(x) for x in sys.argv[1:]]
	r.update(counts)

	print r.p
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.