Commits

Trammell Hudson committed 2d73c51

Ported IK library from C

  • Participants
  • Parent commits 6024e4b

Comments (0)

Files changed (2)

 import time
 import MDC
 
-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
+from Joint import Joint
 
 class PumaFK(object):
 	def __init__(self, joints):
 
 
 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, -90, +8000 - -8000, 0)
-	j5 = PumaJoint(0, 0, 0, -8000 - +8000, 9000/-40000.0)
-	j6 = PumaJoint(50, 0, 0, -6500 - +6500, 1200/-10000.0)
+	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:
+	if len(sys.argv) == 6+1:
 		counts = [int(x) for x in sys.argv[1:]]
 		r.update(counts)
 		print r.p
+#!/usr/bin/python
+# Direct inverse kinesmatics for the PUMA.
+#
+# Based on "A geometric approach in solving the inverse kinematics of
+# PUMA robots" by Lee and Ziegler, 1983.
+#
+# http://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf?sequence=5
+
+import sys
+from math import pi, sin, cos, sqrt, atan2, fabs
+from Joint import Joint
+
+class PumaIK:
+	def __init__(self, right, above, joints):
+		if len(joints) != 6:
+			raise Exception("PUMA must have six joints")
+		self.joints = joints
+		self.theta = [0,0,0,0,0,0]
+		self.counts = [0,0,0,0,0,0]
+
+		if right:
+			self.right = 1
+		else:
+			self.right = -1
+
+		if above:
+			self.above = 1
+		else:
+			self.above = -1
+
+	def major(self, xyz):
+		px = xyz[0]
+		py = xyz[1]
+		pz = xyz[2]
+
+		a2 = self.joints[1].a
+		a3 = self.joints[2].a
+
+		d2 = self.joints[1].d
+		d4 = self.joints[3].d
+
+		if px*px + py*py < d2*d2:
+			print "Out of reach"
+			return False
+
+		r = sqrt(px*px + py*py - d2*d2)
+		R = sqrt(px*px + py*py + pz*pz - d2*d2)
+
+		# theta[0] defined in equation 26
+		self.theta[0] = atan2(
+			-self.right * py * r - px * d2,
+			-self.right * px * r + py * d2
+		)
+
+		# theta[1] is equations 28 - 35.
+		sin_alpha = -pz / R
+		cos_alpha = -self.right * r / R
+		cos_beta = (a2*a2 + R*R - (d4*d4 + a3*a3)) / (2*a2*R)
+		if cos_beta > 1 or cos_beta < -1:
+			print "Beta out of range"
+			return False
+
+		sin_beta = sqrt(1 - cos_beta*cos_beta)
+		sin_t2 = sin_alpha * cos_beta + self.right * self.above * cos_alpha * sin_beta
+		cos_t2 = cos_alpha * cos_beta - self.right * self.above * sin_alpha * sin_beta
+		self.theta[1] = atan2(sin_t2, cos_t2)
+
+		# theta[2] 
+		t2 = d4*d4 + a3*a3
+		t = sqrt(t2)
+		cos_phi = (a2*a2 + t2 - R*R) / (2 * a2 * t)
+		if cos_phi > 1 or cos_phi < -1:
+			print "Phi out of range"
+			return False
+
+		sin_phi = self.right * self.above * sqrt(1 - cos_phi*cos_phi)
+		sin_beta = d4 / t
+		cos_beta = fabs(a3) / t
+		sin_t3 = sin_phi*cos_beta - cos_phi*sin_beta
+		cos_t3 = cos_phi*cos_beta + sin_phi*sin_beta
+		self.theta[2] = atan2(sin_t3, cos_t3)
+
+		return True
+
+	# Compute the wrist commands given the desired wrist configuration.
+	# The major positions must have already been computed to position
+	# the major axes.
+	def wrist(self, a, s, n):
+		M = 1
+		ax = a[0]
+		ay = a[1]
+		az = a[2]
+
+		sx = s[0]
+		sy = s[1]
+		sz = s[2]
+
+		nx = n[0]
+		ny = n[1]
+		nz = n[2]
+
+		C1 = cos(self.theta[0])
+		S1 = sin(self.theta[0])
+		C23 = cos(self.theta[1] + self.theta[2])
+		S23 = sin(self.theta[1] + self.theta[2])
+
+		S4 = M * (C1*ay - S1*ax)
+		C4 = M * (C1*C23*ax + S1*C23*ay- S23*az)
+
+		self.theta[3] = atan2(S4, C4)
+
+		S5 = (C1*C23*C4 - S1*S4)*ax + (S1*C23*C4 + C1*S4)*ay - C4* S23*az
+		C5 = C1*S23*ax + S1*S23*ay + C23*az
+		self.theta[4] = atan2(S5,C5)
+
+		S6 = (-S1*C4 - C1*C23*S4)*nx + (C1*C4-S1*C23*S4)*ny + S4*S23*nz
+		C6 = (-S1*C4-C1*C23*S4)*sx + (C1*C4-S1*C23*S4)*sy + S4*S23*sz
+
+		self.theta[5] = atan2(S6,C6)
+
+		return True
+
+	def get_counts(self):
+		counts = []
+		for i in range(0, 6):
+			j = self.joints[i]
+			c = (self.theta[i] - j.theta) * j.scale / pi
+			if j.coupling:
+				c += counts[i-1] * j.coupling
+			counts.append(int(c))
+		return counts
+
+
+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 = PumaIK(True, True, (j1,j2,j3,j4,j5,j6))
+
+	if len(sys.argv) != 3+1:
+		sys.exit(0)
+
+	pos = [int(x) for x in sys.argv[1:]]
+	r.major(pos)
+	print r.get_counts()