1. Trammell Hudson
  2. puma

Commits

Trammell Hudson  committed 0f6ce87

Precompute scale, try to adjust for tool position

  • Participants
  • Parent commits 2d73c51
  • Branches default

Comments (0)

Files changed (2)

File PumaFK.py

View file
 			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

View file
 # 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()])