Source

puma / Puma.py

Full commit
Trammell Hudson 7c4b3b0 








Trammell Hudson 3e8a230 





Trammell Hudson 7c4b3b0 
























Trammell Hudson 3e8a230 
Trammell Hudson 7c4b3b0 
Trammell Hudson 3e8a230 

Trammell Hudson 7c4b3b0 



Trammell Hudson 3e8a230 




















































Trammell Hudson 7c4b3b0 


Trammell Hudson 3e8a230 
Trammell Hudson 7c4b3b0 

Trammell Hudson 3e8a230 
























#!/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.
#
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):
		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
			self.counts = [int(x) for x in counts]
			#print "rx: " + line
			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:
		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)