finish classes

urosc/oscreceive.py

@@ -0,0 +1,65 @@
+#from helpfunctions import *
+import numpy as np
+import time
+
+class OSCReceive:
+    from pythonosc import dispatcher 
+    from pythonosc import osc_server 
+    
+    import threading 
+    
+
+        
+    def handle_received_osc_joint(self,address, *args): 
+        
+        self.server=None
+        self.server_thread=None
+
+        self.oscreceiveintervals[self.oscreceiveintervals_pos]=time.time()-self.last_oscreceive
+        self.last_oscreceive=time.time()
+        self.oscreceiveintervals_pos+=1
+        self.oscreceiveintervals_pos%=np.size(self.oscreceiveintervals)
+        
+        #print(f"Received OSC message on {address}") 
+        joint_q_aim=[0,0,0,0,0,0]
+        try:
+            for iarg, arg in enumerate(args): 
+                #print(f"Data: {arg}") 
+                joint_q_aim[iarg]=float(arg)
+        except:
+            print("Data not in right format")
+            print("Data: "+str(args)) 
+        else:
+            if self.ur is not None:
+                self.ur.setJointQAim(joint_q_aim)
+
+                
+
+    def getCurrentReceiveFrequency(self):
+        return 1.0/np.mean(self.oscreceiveintervals)
+    
+    def __init__(self,ip,port,_ur=None):
+            
+        #osc receive statistics
+        self.oscreceiveintervals = np.zeros(100)
+        self.oscreceiveintervals_pos=0
+        self.last_oscreceive=0
+
+        self.ur = _ur
+
+
+        # Set up the dispatcher for the server 
+        disp = self.dispatcher.Dispatcher() 
+        disp.map("/joint", self.handle_received_osc_joint)  # You can change '/filter' to any address pattern you expect 
+    
+        # Set up OSC server 
+        self.server = self.osc_server.ThreadingOSCUDPServer((ip, port), disp) 
+        print(f"Serving on {self.server.server_address}") 
+        self.server_thread = self.threading.Thread(target=self.server.serve_forever) 
+        self.server_thread.start() 
+    
+        
+
+    def disconnect(self):
+        self.server.shutdown() 
+        self.server_thread.join() 

urosc/oscsend.py

@@ -29,4 +29,4 @@ class OSCSend:
 
         for c in self.oscreceivers:                
             for jointi,joint in enumerate(q):
-                c.send_message("/j"+str(jointi), toDeg(joint))
+                c.send_message("/j"+str(jointi), toDeg(joint))

urosc/ui.py

@@ -10,8 +10,6 @@ class UI:
         self.debugtext=["","",""]
         self.width=0
         self.height=0
-        self.joint_min=None
-        self.joint_max=None
         self.joint_min=_joint_min
         self.joint_max=_joint_max
         self.frequency_scr=_frequency_scr

urosc/ur.py

@@ -0,0 +1,81 @@
+from helpfunctions import *
+import time
+
+class UR:
+        
+    import rtde_receive
+    import rtde_control
+    
+    def __init__(self,_joint_min,_joint_max,_frequency_rtde_send=500,_frequency_rtde_receive=500):
+        self.frequency_rtde_send=_frequency_rtde_send
+        self.frequency_rtde_receive=_frequency_rtde_receive
+
+        
+        self.last_receivertde=0
+        self.last_sendrtde=0
+
+        self.joint_min=_joint_min
+        self.joint_max=_joint_max
+                
+        self.rtde_r=None
+        self.rtde_c=None
+
+                
+        # Parameters
+        self.velocity = 0.1
+        self.acceleration = 0.1
+        self.lookahead_time = 0.1
+        self.gain = 300
+        self.joint_q_aim=[0,0,0,0,0,0] #joint rotations aiming for
+        self.joint_q = [0,0,0,0,0,0] #joint rotation to be send
+        self.velocitylimit=[1,1,1,3,3,3] #joint velocity limits
+
+
+        self.joint_q_received=[0,0,0,0,0,0]
+
+    def connect(self,robotip):
+        
+        rtde_r = self.rtde_receive.RTDEReceiveInterface(robotip, self.frequency_rtde_receive)
+
+        rtde_c = self.rtde_control.RTDEControlInterface(robotip)
+
+    def setJointQAim(self,q):
+        self.joint_q_aim=q
+
+        
+    def send(self,looptime):
+    
+        if looptime-self.last_sendrtde<1.0/self.frequency_rtde_send:
+            return
+        self.last_sendrtde=looptime
+
+        for i in range(6): #update joint_q with max speeds
+            self.joint_q[i] += constrain(self.joint_q_aim[i]-self.joint_q[i], -self.velocitylimit[i]/self.frequency_rtde_send,self.velocitylimit[i]/self.frequency_rtde_send) #constrain joint speeds
+
+        if self.rtde_c is not None:
+            self.rtde_c.servoJ(self.joint_q, self.velocity, self.acceleration, 1.0/self.frequency_rtde_send, self.lookahead_time, self.gain)
+
+    def receive(self,looptime):
+        
+        #Get robot joint angles
+        if looptime-self.last_receivertde>=1.0/self.frequency_rtde_receive:
+            self.last_receivertde=looptime
+
+            if self.rtde_r is not None:
+                self.joint_q_received=self.rtde_r.getActualQ()
+            else:
+                _offsets=[12.2,5.3,0,6,12.4,612.91]
+                _factors=[1.2,1.14,1.214,0.9921,1,1.092]
+                
+                for _i,_offset in enumerate(_offsets): #generate mockup values
+                    self.joint_q_received[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(self.joint_max[_i]-self.joint_min[_i])+self.joint_min[_i]
+
+    def getReceivedQ(self):
+        return self.joint_q_received
+
+    def disconnect(self):
+        if self.rtde_c is not None:
+            self.rtde_c.servoStop()
+            self.rtde_c.stopScript()
+    
+

urosc/urosc.py

@@ -1,93 +1,39 @@
 import argparse 
-from pythonosc import dispatcher 
-from pythonosc import osc_server 
 
-import threading 
+
 import time
 import math
 import numpy as np
 import curses
 
-import rtde_receive
-import rtde_control
 
 
 from helpfunctions import *
 from ui import UI
 from oscsend import OSCSend
+from oscreceive import OSCReceive
+from ur import UR
 
 
 
-
-
-rtde_r=None
-rtde_c=None
-server=None
-
 #Looptimings
 frequency_loop=500
-frequency_rtde=None 
-
-
-#Robot Control Parameters
-# Parameters
-velocity = 0.1
-acceleration = 0.1
-lookahead_time = 0.1
-gain = 300
-joint_q_aim=[0,0,0,0,0,0] #joint rotations aiming for
-joint_q = [0,0,0,0,0,0] #joint rotation to be send
-velocitylimit=[1,1,1,3,3,3] #joint velocity limits
-
-
-#osc receive statistics
-oscreceiveintervals = np.zeros(100)
-oscreceiveintervals_pos=0
-last_oscreceive=0
-
-def handle_received_osc_joint(address, *args): 
-    oscreceiveintervals[oscreceiveintervals_pos]=time.time()-last_oscreceive
-    last_oscreceive=time.time()
-    oscreceiveintervals_pos+=1
-    oscreceiveintervals_pos%=np.size(oscreceiveintervals)
-    
-    print(f"Received OSC message on {address}") 
-    for iarg, arg in enumerate(args): 
-        #print(f"Data: {arg}") 
-        joint_q_aim[iarg]=float(arg)
-
- 
-def setupOSC(ip, port): 
-    # Set up the dispatcher for the server 
-    disp = dispatcher.Dispatcher() 
-    disp.map("/joint", handle_received_osc_joint)  # You can change '/filter' to any address pattern you expect 
- 
-    # Set up OSC server 
-    server = osc_server.ThreadingOSCUDPServer((ip, port), disp) 
-    print(f"Serving on {server.server_address}") 
-    server_thread = threading.Thread(target=server.serve_forever) 
-    server_thread.start() 
- 
-    return server,server_thread
 
 
 
-def setupRTDE(robotip,frequency):
-    dt = 1 / frequency
-    rtde_r = rtde_receive.RTDEReceiveInterface(robotip, frequency)
 
-    rtde_c = rtde_control.RTDEControlInterface(robotip)
 
-    return rtde_r
+
- 
  
 joint_min=[toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(0)]
 joint_max=[toRad(360),toRad(360),toRad(360),toRad(360),toRad(360),toRad(360)]
 
 
 ui = UI(joint_min,joint_max)
-oscsend = OSCSend()
 
+oscsend = OSCSend()
+oscreceive = None
+ur = UR(joint_min,joint_max)
 
 
 def main(stdscr):
@@ -103,12 +49,8 @@ def main(stdscr):
     loopcounter = 0
     loopdurations = np.zeros(100)
     loopdurations_pos=0
-    last_receivertde=0
-    last_sendrtde=0
     
 
-    q=[0,0,0,0,0,0]
-
     
     try:
         while ui.getKey() != ord('q'):
@@ -116,36 +58,16 @@ def main(stdscr):
             
             start = time.time()
             
-
+            ur.receive(looptime)
-            #Get robot joint angled
-            if looptime-last_receivertde>=1.0/frequency_receivertde:
-                last_receivertde=looptime
-
-                if rtde_r is not None:
-                    q=rtde_r.getActualQ()
-                else:
-                    _offsets=[12.2,5.3,0,6,12.4,612.91]
-                    _factors=[1.2,1.14,1.214,0.9921,1,1.092]
-                    
-                    for _i,_offset in enumerate(_offsets):
-                        q[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(joint_max[_i]-joint_min[_i])+joint_min[_i]
-
 
             #send data to all osc receivers
-            oscsend.send(looptime,q)
+            oscsend.send(looptime,ur.getReceivedQ())
-
-            if looptime-last_sendrtde>=1.0/frequency_sendrtde:
-                last_sendrtde=looptime
-
-                for i in range(6): #update joint_q with max speeds
-                    joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -velocitylimit[i]/frequency_sendrtde,velocitylimit[i]/frequency_sendrtde) #constrain joint speeds
-
-                if rtde_c is not None:
-                    rtde_c.servoJ(joint_q, velocity, acceleration, 1.0/frequency_sendrtde, lookahead_time, gain)
 
+            if ur is not None:
+                ur.send(looptime)
 
             #Display
-            ui.update(q,looptime)
+            ui.update(ur.getReceivedQ(),looptime)
                 
             #Looptiming
             end = time.time()
@@ -156,7 +78,8 @@ def main(stdscr):
 
             ui.setDebugtext(0,"loopcounter="+str(loopcounter))
             ui.setDebugtext(1,"loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)))
-            ui.setDebugtext(2,"osc receive frequency="+str(round(1.0/np.mean(oscreceiveintervals),0))+" \t pos="+str(oscreceiveintervals_pos))
+            if oscreceive is not None:
+                ui.setDebugtext(2,"osc receive frequency="+str(round(oscreceive.getCurrentReceiveFrequency(),0)))
 
             
             
@@ -190,6 +113,7 @@ if __name__ == "__main__":
     parser.add_argument("--test", action='store_true', default=False, help="Disable network and robot communication")
     
     
+    '''
     parser.add_argument(
         "-fs",
         "--frequencysend",
@@ -207,29 +131,29 @@ if __name__ == "__main__":
         type=float,
         default=500.0,
         metavar="<frequency>")
+    '''
     
     args = parser.parse_args()
 
-    frequency_receivertde=args.frequencyreceive
-    frequency_sendrtde=args.frequencysend
 
+    
  
     
     if args.test is not True:
-        server,server_thread=setupOSC(args.listenip, args.listenport) 
+        ur.connect(args.robotip)
+
+        oscreceive = OSCReceive(args.listenip, args.listenport,ur)    
 
         oscsend.setupReceivers([args.clientip],args.port)
 
-        rtde_r=setupRTDE(args.robotip,frequency_rtde)
+        
 
     
     curses.wrapper(main)
 
-    if server is not None:
+
-        input("Ended. Press Enter to shutdown")
+    if oscreceive is not None:
-        server.shutdown() 
+        oscreceive.disconnect()
-        server_thread.join() 
+    if ur is not None:
-    if rtde_c is not None:
+        ur.disconnect()
-        rtde_c.servoStop()
-        rtde_c.stopScript()