Source

puma / Puma.py

Diff from to
 # Talks to the MDC network socket to get the state of the
 # controllers.  Maps to FK position, commands via IK, etc.
 #
-from PumaFK import PumaFK
-from PumaIK import PumaIK
 import socket
 import threading
 import time
+import os
+import re
+import sys
+from PumaFK import PumaFK
+from PumaIK import PumaIK
+from math import sqrt
 
 class Puma:
 	def __init__(self, con, joints):
 			if len(counts) != 6:
 				print "Parse error: " + line
 				continue
+			self.counts = [int(x) for x in counts]
 			#print "rx: " + line
-			self.fk.update([int(x) for x in counts])
+			self.fk.update(self.counts)
+			#print [int(x) for x in r.fk.p]
 
 		print "RX THREAD EXITED"
 		self.live = False
 
+	# Move immediately to a set of counts
+	def move_raw(self, ms, counts):
+		cmd = "go," + str(int(ms)) + "," + ",".join([str(x) for x in counts])
+		print cmd
+		self.sock.send(cmd + "\r\n")
+
+	def home(self):
+		self.move_raw(2000, [0,0,0,0,0,0])
+
+	# Move immediately to an explicit IK computed position
+	def move_ik(self, ms, xyz, a, s, n):
+		self.ik.ik(xyz, a, s, n)
+		self.move_raw(ms, self.ik.get_counts())
+
+	# Move linearly to an IK computed position at mm/s speed
+	# This will hold the current tool configuration
+	def move_to(self, vel, dst_xyz):
+		xyz = self.fk.p[:]
+		cur_a = self.fk.a[:]
+		cur_s = self.fk.s[:]
+		cur_n = self.fk.n[:]
+
+		dx = dst_xyz[0] - xyz[0]
+		dy = dst_xyz[1] - xyz[1]
+		dz = dst_xyz[2] - xyz[2]
+		dist = sqrt(dx*dx + dy*dy + dz*dz)
+
+		steps_per_sec = 10
+		steps = int(dist * steps_per_sec / vel)
+		dt = 1.0 / steps_per_sec
+
+		print "dist=" + str(int(dist))
+		print "steps=" + str(steps)
+		print "dt=" + str(round(dt, 4))
+
+		dx /= steps
+		dy /= steps
+		dz /= steps
+
+		print "dx=" + str(round(dx, 2))
+		print "dy=" + str(round(dy, 2))
+		print "dz=" + str(round(dz, 2))
+
+		#dt = 0.
+
+		for i in range(0,steps):
+			xyz[0] += dx
+			xyz[1] += dy
+			xyz[2] += dz
+			self.move_ik(dt*1000, xyz, cur_a, cur_s, cur_n)
+			time.sleep(dt)
+
+
 if __name__ == "__main__":
 	import PumaConfig
 	r = Puma(("localhost", 31415), PumaConfig.joints)
+	vel = 20
 
 	while r.live:
-		print r.fk.p
-		time.sleep(1)
+		line = sys.stdin.readline()
+		if not line:
+			break
+		line = line.strip()
+
+		if line == "home":
+			r.home()
+			continue
+		if line == "hold":
+			r.move_raw(1000, r.counts)
+			continue
+		if line == "r":
+			print r.counts
+			continue
+		if line == "p":
+			print [int(x) for x in r.fk.p], [round(x,2) for x in r.fk.a], [round(x,2) for x in r.fk.s], [round(x,2) for x in r.fk.n]
+			continue
+		g = re.match("^v ([0-9]+)", line)
+		if g is not None:
+			vel = int(g.group(1))
+			continue
+
+		g = re.match("^(-?[0-9]+ -?[0-9]+ -?[0-9]+)$", line)
+		if g is not None:
+			xyz = [int(x) for x in g.group(1).split(" ")]
+			r.move_to(vel, xyz)