From 9127b21d6dc46f3fdd756a36102dc72e371e3dd6 Mon Sep 17 00:00:00 2001
From: Bart Moyaers <bart.moyaers@kuleuven.be>
Date: Tue, 19 Nov 2019 09:44:13 +0100
Subject: [PATCH 1/2] Change speedx method to RT data stream

---
 urx/urrobot.py | 11 ++++++++++-
 urx/urrtmon.py | 26 ++++++++++++++++++++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/urx/urrobot.py b/urx/urrobot.py
index 94c87f5..f5fa501 100644
--- a/urx/urrobot.py
+++ b/urx/urrobot.py
@@ -86,6 +86,15 @@ def send_program(self, prog):
         self.logger.info("Sending program: " + prog)
         self.secmon.send_program(prog)
 
+    def send_custom_program(self, prog):
+        """
+        send a complete program using urscript to the robot
+        the program is executed immediatly and any runnning
+        program is interrupted
+        """
+        self.logger.info("Sending program: " + prog)
+        self.rtmon.send_program(prog)
+
     def get_tcp_force(self, wait=True):
         """
         return measured force in TCP
@@ -264,7 +273,7 @@ def speedx(self, command, velocities, acc, min_time):
         vels.append(acc)
         vels.append(min_time)
         prog = "{}([{},{},{},{},{},{}], a={}, t_min={})".format(command, *vels)
-        self.send_program(prog)
+        self.send_custom_program(prog)
 
     def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None):
         """
diff --git a/urx/urrtmon.py b/urx/urrtmon.py
index 9c08133..d9141fe 100644
--- a/urx/urrtmon.py
+++ b/urx/urrtmon.py
@@ -10,6 +10,7 @@
 import time
 import threading
 from copy import deepcopy
+from urx.ursecmon import Program
 
 import numpy as np
 
@@ -55,6 +56,8 @@ def __init__(self, urHost):
         self._buffer = []
         self._csys = None
         self._csys_lock = threading.Lock()
+        self._prog_queue = []
+        self._prog_queue_lock = threading.Lock()
 
     def set_csys(self, csys):
         with self._csys_lock:
@@ -195,6 +198,28 @@ def __recv_rt_data(self):
         with self._dataEvent:
             self._dataEvent.notifyAll()
 
+    def __send_rt_data(self):
+        with self._prog_queue_lock:
+            while len(self._prog_queue) > 0:
+                data = self._prog_queue.pop()
+                self._rtSock.send(data.program)
+                with data.condition:
+                    data.condition.notify_all()
+
+    def send_program(self, prog):
+        """
+        send program to robot in URRobot format
+        If another program is send while a program is running the first program is aborded.
+        """
+        prog.strip()
+        if not isinstance(prog, bytes):
+            prog = prog.encode()
+        data = Program(prog + b"\n")
+        with data.condition:
+            with self._prog_queue_lock:
+                self._prog_queue.append(data)
+            data.condition.wait()
+
     def start_buffering(self):
         """
         start buffering all data from controller
@@ -260,4 +285,5 @@ def run(self):
         self._rtSock.connect((self._urHost, 30003))
         while not self._stop_event:
             self.__recv_rt_data()
+            self.__send_rt_data()
         self._rtSock.close()

From 09fe6993667f4515febde8a0b7bd5898dad6bffa Mon Sep 17 00:00:00 2001
From: Bart Moyaers <bart.moyaers@gmail.com>
Date: Wed, 11 Mar 2020 09:36:25 +0100
Subject: [PATCH 2/2] correct speedj argument error on newer UR versions

---
 urx/urrobot.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/urx/urrobot.py b/urx/urrobot.py
index f5fa501..8d3579b 100644
--- a/urx/urrobot.py
+++ b/urx/urrobot.py
@@ -272,7 +272,7 @@ def speedx(self, command, velocities, acc, min_time):
         vels = [round(i, self.max_float_length) for i in velocities]
         vels.append(acc)
         vels.append(min_time)
-        prog = "{}([{},{},{},{},{},{}], a={}, t_min={})".format(command, *vels)
+        prog = "{}([{},{},{},{},{},{}],{},{})".format(command, *vels)
         self.send_custom_program(prog)
 
     def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None):