# puma

committed 84af5a0

Started on a FK library in Python

# 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`
`+`
`+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.