From 387bdaa178649e94389162596eb06c86739d6754 Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Fri, 23 Mar 2012 18:31:36 -0700
Subject: [PATCH] Adding UR5 driver package (in progress)
---
universal_robot_driver/driver.py | 86 +++++++++++++++++++++++++++++
universal_robot_driver/manifest.xml | 14 +++++
universal_robot_driver/prog | 51 +++++++++++++++++
3 files changed, 151 insertions(+)
create mode 100755 universal_robot_driver/driver.py
create mode 100644 universal_robot_driver/manifest.xml
create mode 100644 universal_robot_driver/prog
diff --git a/universal_robot_driver/driver.py b/universal_robot_driver/driver.py
new file mode 100755
index 0000000..f7af8e0
--- /dev/null
+++ b/universal_robot_driver/driver.py
@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+import roslib; roslib.load_manifest('universal_robot_driver')
+import time, sys
+import socket
+import struct
+import SocketServer
+
+HOSTNAME='ur-xx'
+HOSTNAME="10.0.1.20"
+PORT=30001
+
+MSG_OUT = 1
+MSG_QUIT = 2
+MSG_JOINT_STATES = 3
+MULT_jointstate = 1000.0
+
+class EOF(Exception): pass
+
+class CommanderTCPHandler(SocketServer.BaseRequestHandler):
+ def recv_more(self):
+ more = self.request.recv(4096)
+ if not more:
+ raise EOF()
+ return more
+
+ def handle(self):
+ print "Handling a request"
+ try:
+ buf = self.recv_more()
+ if not buf: return
+
+ while True:
+ print "Buf:", [ord(b) for b in buf]
+
+ # Unpacks the message type
+ mtype = struct.unpack_from("!i", buf, 0)[0]
+ buf = buf[4:]
+ print "Message type:", mtype
+
+ if mtype == MSG_OUT:
+ # Unpacks string message, terminated by tilde
+ i = buf.find("~")
+ while i < 0:
+ buf = buf + self.recv_more()
+ i = buf.find("~")
+ if len(buf) > 2000:
+ raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
+ s, buf = buf[:i], buf[i+1:]
+ print "Out: %s" % s
+ elif mtype == MSG_QUIT:
+ print "Quitting"
+ sys.exit(0)
+ elif mtype == MSG_JOINT_STATES:
+ while len(buf) < 3*(6*4):
+ buf = buf + self.recv_more()
+ state_mult = struct.unpack_from("!" + ("i"*3*6), buf, 0)
+ buf = buf[3*6*4:]
+ state = [s / MULT_jointstate for s in state_mult]
+ print "Joint state:", state
+ else:
+ raise Exception("Unknown message type: %i" % mtype)
+
+ if not buf:
+ buf = buf + self.recv_more()
+ except EOF:
+ print "Connection closed"
+
+
+class TCPServer(SocketServer.TCPServer):
+ allow_reuse_address = True
+ timeout = 5
+
+sock = None
+def main():
+ global sock
+
+ # Sends the program to the robot
+ sock = socket.create_connection((HOSTNAME, PORT))
+ with open('prog') as fin:
+ sock.sendall(fin.read())
+
+ server = TCPServer(("", 50001), CommanderTCPHandler)
+ server.handle_request()
+
+
+if __name__ == '__main__': main()
diff --git a/universal_robot_driver/manifest.xml b/universal_robot_driver/manifest.xml
new file mode 100644
index 0000000..505d94b
--- /dev/null
+++ b/universal_robot_driver/manifest.xml
@@ -0,0 +1,14 @@
+<package>
+ <description brief="universal_robot_driver">
+
+ universal_robot_driver
+
+ </description>
+ <author>Stuart Glaser</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/universal_robot_driver</url>
+
+</package>
+
+
diff --git a/universal_robot_driver/prog b/universal_robot_driver/prog
new file mode 100644
index 0000000..46e1da3
--- /dev/null
+++ b/universal_robot_driver/prog
@@ -0,0 +1,51 @@
+def driverProg():
+ MSG_OUT = 1
+ MSG_QUIT = 2
+ MSG_JOINT_STATES = 3
+ MULT_jointstate = 1000
+
+ def send_joint_state():
+ q = get_joint_positions()
+ qdot = get_joint_speeds()
+ tau = get_joint_torques()
+ socket_send_int(MSG_JOINT_STATES)
+ socket_send_int(floor(MULT_jointstate * q[0]))
+ socket_send_int(floor(MULT_jointstate * q[1]))
+ socket_send_int(floor(MULT_jointstate * q[2]))
+ socket_send_int(floor(MULT_jointstate * q[3]))
+ socket_send_int(floor(MULT_jointstate * q[4]))
+ socket_send_int(floor(MULT_jointstate * q[5]))
+ socket_send_int(floor(MULT_jointstate * qdot[0]))
+ socket_send_int(floor(MULT_jointstate * qdot[1]))
+ socket_send_int(floor(MULT_jointstate * qdot[2]))
+ socket_send_int(floor(MULT_jointstate * qdot[3]))
+ socket_send_int(floor(MULT_jointstate * qdot[4]))
+ socket_send_int(floor(MULT_jointstate * qdot[5]))
+ socket_send_int(floor(MULT_jointstate * tau[0]))
+ socket_send_int(floor(MULT_jointstate * tau[1]))
+ socket_send_int(floor(MULT_jointstate * tau[2]))
+ socket_send_int(floor(MULT_jointstate * tau[3]))
+ socket_send_int(floor(MULT_jointstate * tau[4]))
+ socket_send_int(floor(MULT_jointstate * tau[5]))
+ end
+
+ socket_open("10.0.2.97", 50001)
+ socket_send_int(1)
+ socket_send_string("hello~")
+ send_joint_state()
+ #socket_send_int(q[0]
+ #M = 1000
+
+ movej([2.2,0,-1.57,0,0,0],2)
+ movej([1.5,0,-1.57,0,0,0],2)
+
+ #i = 0
+ #while i < 5:
+ # movej([2.2,0,-1.57,0,0,0],2)
+ # movej([1.5,0,-1.57,0,0,0],2)
+ # i = i + 1
+ #end
+
+ socket_send_int(2)
+end
+driverProg()
--
GitLab