Source

puma / PumaIK.py

Trammell Hudson 2d73c51 






Trammell Hudson 0f6ce87 



Trammell Hudson 2d73c51 






















Trammell Hudson 2abaa2e 
Trammell Hudson 0f6ce87 




Trammell Hudson 45a8704 
Trammell Hudson 0f6ce87 


Trammell Hudson 45a8704 
Trammell Hudson 0f6ce87 

Trammell Hudson 2abaa2e 
Trammell Hudson 0f6ce87 



Trammell Hudson 2d73c51 




























Trammell Hudson 0f6ce87 
Trammell Hudson 2d73c51 










Trammell Hudson 0f6ce87 
Trammell Hudson 2d73c51 



















































Trammell Hudson 0f6ce87 





Trammell Hudson 2d73c51 






Trammell Hudson 0385ad0 

Trammell Hudson 2d73c51 




Trammell Hudson 45a8704 





Trammell Hudson 2abaa2e 
Trammell Hudson 0f6ce87 

Trammell Hudson 2abaa2e 
#!/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):
		counts = []
		for i in range(0, 6):
			j = self.joints[i]
			a = self.theta[i] - j.theta
			if a > pi:
				a -= 2 * pi
			elif a < -pi:
				a += 2 * pi
			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()])