puma /

# 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): = True
		self.joints = joints
		self.counts = [0,0,0,0,0] = PumaFK(joints)
		self.ik = PumaIK(True, True, joints)
		self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		self.con = self.sock.makefile()
		self.thread = threading.Thread(target = self.rx_loop)
		self.thread.daemon = True

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

		print "RX THREAD EXITED" = 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 =[:]
		cur_a =[:]
		cur_s =[:]
		cur_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)

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

		line = sys.stdin.readline()
		if not line:
		line = line.strip()

		if line == "home":
		if line == "hold":
			r.move_raw(1000, r.counts)
		if line == "r":
			print r.counts
		if line == "p":
			print [int(x) for x in], [round(x,2) for x in], [round(x,2) for x in], [round(x,2) for x in]
		g = re.match("^v ([0-9]+)", line)
		if g is not None:
			vel = int(

		g = re.match("^(-?[0-9]+ -?[0-9]+ -?[0-9]+)$", line)
		if g is not None:
			xyz = [int(x) for x in" ")]
			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
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.