# Commits

committed 0f6ce87

Precompute scale, try to adjust for tool position

• Participants
• Parent commits 2d73c51
• Branches default

# File PumaFK.py

` 			count = counts[i]`
` 			if j.coupling:`
` 				count -= counts[i-1] * j.coupling`
`-			angle = counts[i] * pi / j.scale + j.theta`
`+			angle = counts[i] / j.scale + j.theta`
` 			self.angles[i] = angle`
` `
` 		c1 = cos(self.angles[0])`
` 	if len(sys.argv) == 6+1:`
` 		counts = [int(x) for x in sys.argv[1:]]`
` 		r.update(counts)`
`-		print r.p`
`+		print [int(x) for x in r.p]`
` 		sys.exit(0)`
` `
` 	#sys.exit(-1)`

# File PumaIK.py

` # 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`
` 		else:`
` 			self.above = -1`
` `
`+	def ik(self, xyz, n, s, a):`
`+		# 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 = xyz`
`+		new_xyz[0] -= d6 * a[0]`
`+		new_xyz[1] -= d6 * a[1]`
`+		new_xyz[2] -= d6 * a[2]`
`+		print "new xyz: " + str(new_xyz)`
`+`
`+		self.major(new_xyz)`
`+		self.wrist(n, s, a)`
`+`
`+	# 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]`
` 		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`
`+			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`
` 		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`
`+			raise Exception("Phi out of range")`
` `
` 		sin_phi = self.right * self.above * sqrt(1 - cos_phi*cos_phi)`
` 		sin_beta = d4 / t`
` 		counts = []`
` 		for i in range(0, 6):`
` 			j = self.joints[i]`
`-			c = (self.theta[i] - j.theta) * j.scale / pi`
`+			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))`
` 		sys.exit(0)`
` `
` 	pos = [int(x) for x in sys.argv[1:]]`
`-	r.major(pos)`
`-	print r.get_counts()`
`+	a = [-1, 0, 0]`
`+	s = [0, -1, 0]`
`+	n = [0, 0, 1]`
`+	r.ik(pos, n, s, a)`
`+	print " ".join([str(int(x*180/pi)) for x in r.theta])`
`+	print " ".join([str(x) for x in r.get_counts()])`