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.
#
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)
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.