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
#
# A is the approach vector of the tool
# S is the sliding vector (the direction the tool would open)
# N is the normal to those two

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 ik(self, xyz, a, s, n):
		# Back project the position of the spherical ball joint
		# by the final dimension of the tool aligned along the
		# approach angle.
		j6 = self.joints[5]
		d6 = j6.d
		new_xyz = list(xyz)
		new_xyz[0] -= d6 * a[0]
		new_xyz[1] -= d6 * a[1]
		new_xyz[2] -= d6 * a[2]
		print "new xyz: " + ",".join([str(int(x)) for x in new_xyz])

		self.major(new_xyz)
		self.wrist(a, s, n)

	# Compute the major axis commands to position the spherical
	# joint at position XYZ, updating the first three elements of
	# theta[].
	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:
			raise Exception("Beta out of range")

		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:
			raise Exception("Phi out of range")

		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, current_theta):
		return self.angle2count(self.theta, current_theta)

	def angle2count(self, dest_theta, current_theta):
		counts = []
		for i in range(0, 6):
			# Adjust by the home position offset to the
			# joint-frame angle.
			j = self.joints[i]
			a = dest_theta[i]

			# Elbow joint is off by 90 deg
			if i == 2 and a < 0:
				a += 2 * pi

			# try adjusting to avoid a wrap around
			# on the wrist joints that can do more than
			# 360 degrees.
			if False and (i == 3 or i == 5):
				if a - current_theta[i] > 1.5*pi:
					a -= 2 * pi
				elif a - current_theta[i] < -1.5*pi:
					a += 2 * pi

			# Adjust based on the joint configuration
			a -= j.theta

			# Convert to a count
			c = a * j.scale
			if j.coupling:
				c += counts[i-1] * j.coupling
			counts.append(int(c))
		return counts


if __name__ == "__main__":
	import PumaConfig
	r = PumaIK(True, True, PumaConfig.joints)

	if len(sys.argv) != 3+1:
		sys.exit(0)

	pos = [int(x) for x in sys.argv[1:]]
	#a = [0, 1, 0]
	#s = [-1, 0, 0]
	#n = [0, 0, -1]
	a = [0.03, 0.99, -0.13]
	s = [-0.93, -0.02, -0.38]
	n = [0.38, -0.13, -0.91]
	r.ik(pos, a, s, n)
	print " ".join([str(int(x*180/pi)) for x in r.theta])
	#print " ".join([str(x) for x in r.get_counts()])
	#print "go,10000," + ",".join([str(x) for x in r.get_counts()])
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.