Source

puma / Puma.py

#!/usr/bin/python
#
# Puma robot arm class.
# 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

class Puma:
	def __init__(self, con, joints):
		self.live = True
		self.joints = joints
		self.counts = [0,0,0,0,0]
		self.fk = PumaFK(joints)
		self.ik = PumaIK(True, True, joints)
		self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		self.sock.connect(con)
		self.con = self.sock.makefile()
		self.thread = threading.Thread(target = self.rx_loop)
		self.thread.daemon = True
		self.thread.start()

	def rx_loop(self):
		while True:
			line = self.con.readline()
			if not line:
				break
			line = line.strip()
			counts = line.split(",")
			if len(counts) != 6:
				print "Parse error: " + line
				continue
			#print "rx: " + line
			self.fk.update([int(x) for x in counts])

		print "RX THREAD EXITED"
		self.live = False

if __name__ == "__main__":
	import PumaConfig
	r = Puma(("localhost", 31415), PumaConfig.joints)

	while r.live:
		print r.fk.p
		time.sleep(1)