Source

puma / PumaIK.py

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