Sending some movej commands

ur5_driver/driver.py

@@ -14,11 +14,18 @@ PORT=30001
 MSG_OUT = 1
 MSG_QUIT = 2
 MSG_JOINT_STATES = 3
+MSG_MOVEJ = 4
 MULT_jointstate = 1000.0
+MULT_time = 1000000.0
+MULT_blend = 1000.0
 
 JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
                'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
 
+Q1 = [2.2,0,-1.57,0,0,0]
+Q2 = [1.5,0,-1.57,0,0,0]
+  
+
 connected_robot = None
 connected_robot_lock = threading.Lock()
 connected_robot_cond = threading.Condition(connected_robot_lock)
@@ -104,6 +111,14 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
         with self.socket_lock:
             self.request.send(struct.pack("!i", MSG_QUIT))
             
+    def send_movej(self, q, a=3, v=0.75, t=0, r=0):
+        assert(len(q) == 6)
+        buf = ''.join([struct.pack("!i", MSG_MOVEJ),
+                       struct.pack("!iiiiii", *[MULT_jointstate * qq for qq in q]),
+                       struct.pack("!ii", MULT_jointstate * a, MULT_jointstate * v),
+                       struct.pack("!ii", MULT_time * t, MULT_blend * r)])
+        with self.socket_lock:
+            self.request.send(buf)
 
 class TCPServer(SocketServer.TCPServer):
     allow_reuse_address = True  # Allows the program to restart gracefully on crash
@@ -135,7 +150,11 @@ def main():
     r = getConnectedRobot(wait=True)
     print "Robot connected"
 
-    time.sleep(1)
+    r.send_movej(Q1)
+    time.sleep(2)
+    r.send_movej(Q2)
+    time.sleep(3)
+    
     print "Sending quit"
     r.send_quit()
 

ur5_driver/prog

@@ -3,8 +3,11 @@ def driverProg():
   MSG_OUT = 1
   MSG_QUIT = 2
   MSG_JOINT_STATES = 3
+  MSG_MOVEJ = 4
   MULT_jointstate = 1000
-
+  MULT_time = 1000000.0
+  MULT_blend = 1000.0
+  
   def send_out(msg):
     enter_critical
     socket_send_int(1)
@@ -57,11 +60,11 @@ def driverProg():
 
   thread_state = run statePublisherThread()
   
-  movej([2.2,0,-1.57,0,0,0],2)
-  movej([1.5,0,-1.57,0,0,0],2)
+  #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:
+  while i < 100:
     send_out("Listening")
     ll = socket_read_binary_integer(1)
 
@@ -74,6 +77,27 @@ def driverProg():
       if mtype == MSG_QUIT:
         send_out("Received QUIT")
         break
+      elif mtype == MSG_MOVEJ:
+        send_out("Received movej")
+	params_mult = socket_read_binary_integer(6+4)
+	if params_mult[0] == 0:
+	  send_out("Received no parameters for movej message")
+	end
+
+	# Unpacks the parameters
+	q = [params_mult[1] / MULT_jointstate,
+             params_mult[2] / MULT_jointstate,
+             params_mult[3] / MULT_jointstate,
+             params_mult[4] / MULT_jointstate,
+             params_mult[5] / MULT_jointstate,
+             params_mult[6] / MULT_jointstate]
+	a = params_mult[7] / MULT_jointstate
+	v = params_mult[8] / MULT_jointstate
+	t = params_mult[9] / MULT_time
+	r = params_mult[10] / MULT_blend
+
+	# Sends the command
+	movej(q, a, v, t, r)
       else:
         send_out("Received unknown message type")
       end