Adding UR5 driver package (in progress)

universal_robot_driver/driver.py

+#!/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()

universal_robot_driver/manifest.xml

+<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>
+
+

universal_robot_driver/prog

+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()