Trammell Hudson avatar Trammell Hudson committed 84af5a0

Started on a FK library in Python

Comments (0)

Files changed (1)

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