implement enable disable control via keyboard

urosc/oscreceive.py

@@ -1,4 +1,4 @@
-#from helpfunctions import *
+from helpfunctions import *
 import numpy as np
 import time
 
@@ -10,7 +10,8 @@ class OSCReceive:
     
 
         
-    def handle_received_osc_joint(self,address, *args): 
+    def handle_received_osc_joint(self,address, *args):
+
         
         self.server=None
         self.server_thread=None
@@ -25,7 +26,7 @@ class OSCReceive:
         try:
             for iarg, arg in enumerate(args): 
                 #print(f"Data: {arg}") 
-                joint_q_aim[iarg]=float(arg)
+                joint_q_aim[iarg]=toRad(float(arg))
         except:
             print("Data not in right format")
             print("Data: "+str(args)) 
@@ -36,7 +37,11 @@ class OSCReceive:
                 
 
     def getCurrentReceiveFrequency(self):
-        return 1.0/np.mean(self.oscreceiveintervals)
+        _mean=np.mean(self.oscreceiveintervals)
+        if _mean>0:
+            return 1.0/_mean
+        else:
+            return 0
     
     def __init__(self,ip,port,_ur=None):
             
@@ -50,7 +55,7 @@ class OSCReceive:
 
         # 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 
+        disp.map("/joints", 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) 
@@ -61,5 +66,7 @@ class OSCReceive:
         
 
     def disconnect(self):
-        self.server.shutdown() 
+        if self.server is not None:
-        self.server_thread.join() 
+            self.server.shutdown() 
+        if self.server_thread is not None:
+            self.server_thread.join() 

urosc/ur.py

@@ -10,6 +10,7 @@ class UR:
         self.frequency_rtde_send=_frequency_rtde_send
         self.frequency_rtde_receive=_frequency_rtde_receive
 
+        self.controlEnabled=False
         
         self.last_receivertde=0
         self.last_sendrtde=0
@@ -27,48 +28,85 @@ class UR:
         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.joint_q = [0,0,0,0,0,0] #joint rotation last sent
-        self.velocitylimit=[1,1,1,3,3,3] #joint velocity limits
+        #self.velocitylimit=[1,1,1,3,3,3] #joint velocity limits
+        self.velocitylimit=[0.2,0.2,0.2,0.2,0.2,0.2] #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)
+        self.rtde_r = self.rtde_receive.RTDEReceiveInterface(robotip, self.frequency_rtde_receive)
 
-        rtde_c = self.rtde_control.RTDEControlInterface(robotip)
+        self.rtde_c = self.rtde_control.RTDEControlInterface(robotip)
 
     def setJointQAim(self,q):
         self.joint_q_aim=q
 
+    def isJointsWithinSafetyLimits(self,q):
+        return self.rtde_c.isJointsWithinSafetyLimits(q)
+
+    def enableControl(self):
+        if time.time()-self.last_receivertde<0.1: #if received position is recent
+            self.joint_q=self.joint_q_received #update current position to received position
+            self.joint_q_aim=self.joint_q_received
+            self.controlEnabled=True
+    def disableControl(self):
+        self.controlEnabled=False
         
+    def isControlEnabled(self):
+        return self.controlEnabled
+    
     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
+        if self.controlEnabled:
-            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
+            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:
+            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)
+                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):
+    def receiveJoints(self,looptime):
         
         #Get robot joint angles
-        if looptime-self.last_receivertde>=1.0/self.frequency_rtde_receive:
+        if looptime-self.last_receivertde<1.0/self.frequency_rtde_receive:
-            self.last_receivertde=looptime
+            return
+        self.last_receivertde=looptime
 
-            if self.rtde_r is not None:
+        if self.rtde_r is not None:
-                self.joint_q_received=self.rtde_r.getActualQ()
+            self.joint_q_received=self.rtde_r.getActualQ()
-            else:
+        else:
-                _offsets=[12.2,5.3,0,6,12.4,612.91]
+            _offsets=[12.2,5.3,0,6,12.4,612.91]
-                _factors=[1.2,1.14,1.214,0.9921,1,1.092]
+            _factors=[1.2,1.14,1.214,0.9921,1,1.092]
-                
+            
-                for _i,_offset in enumerate(_offsets): #generate mockup values
+            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]
+                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 receiveStatus(self,looptime):
+        
+        if looptime-self.last_receivertde<1.0/self.frequency_rtde_receive:
+            return
+        self.last_receivertde=looptime
+        
+        self.status_connected=self.rtde_r.isConnected()
+        self.status_timestamp=self.rtde_r.getTimestamp()
+        self.status_protectivestopped=self.rtde_r.isProtectiveStopped()
+        self.status_emergencystopped=self.rtde_r.isEmergencyStopped()
+        self.status_robotstatus=self.rtde_c.getRobotStatus()
+        '''
+        self.status_=self.rtde_r.()
+        self.status_=self.rtde_r.()
+        self.status_=self.rtde_r.()
+        self.status_=self.rtde_r.()
+        self.status_=self.rtde_r.()
+        self.status_=self.rtde_r.()
+        self.status_=self.rtde_r.()
+        '''
 
     def getReceivedQ(self):
         return self.joint_q_received
@@ -78,4 +116,5 @@ class UR:
             self.rtde_c.servoStop()
             self.rtde_c.stopScript()
     
+    
 

urosc/urosc.py

@@ -15,7 +15,6 @@ from oscreceive import OSCReceive
 from ur import UR
 
 
-
 #Looptimings
 frequency_loop=500
 
@@ -50,7 +49,8 @@ def main(stdscr):
     loopdurations = np.zeros(100)
     loopdurations_pos=0
     
-
+    
+    keyDown=False
     
     try:
         while ui.getKey() != ord('q'):
@@ -58,7 +58,8 @@ def main(stdscr):
             
             start = time.time()
             
-            ur.receive(looptime)
+            ur.receiveJoints(looptime)
+            ur.receiveStatus(looptime)
 
             #send data to all osc receivers
             oscsend.send(looptime,ur.getReceivedQ())
@@ -76,11 +77,20 @@ def main(stdscr):
             loopdurations_pos+=1
             loopdurations_pos%=np.size(loopdurations)
 
-            ui.setDebugtext(0,"loopcounter="+str(loopcounter))
+            ui.setDebugtext(0,"loopcounter="+str(loopcounter)+"\tControlEnabled="+str(ur.isControlEnabled()))
             ui.setDebugtext(1,"loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)))
             if oscreceive is not None:
                 ui.setDebugtext(2,"osc receive frequency="+str(round(oscreceive.getCurrentReceiveFrequency(),0)))
-
+            
+            if ui.getKey() == ord('e') and keyDown==False:
+                keyDown=True
+                ur.enableControl()
+            if ui.getKey() == ord('d') and keyDown==False:
+                keyDown=True
+                ur.disableControl()
+            if ui.getKey()==-1:
+                keyDown=False
+            
             
             
 
@@ -88,9 +98,10 @@ def main(stdscr):
                 time.sleep(loop_dt - loopduration)
             loopcounter += 1
 
-    except KeyboardInterrupt:
+    except (KeyboardInterrupt):        
-        
         print("\nKeyboardInterrupt")
+    except Exception as e:
+        pass
     curses.endwin()
         
 
@@ -101,9 +112,9 @@ if __name__ == "__main__":
     parser.add_argument("--listenport",
         type=int, default=5005, help="The port to listen on")
 
-    parser.add_argument("--clientip",
+    parser.add_argument("--oscsendip",
         default="127.0.0.1", help="The ip to connect to")
-    parser.add_argument("--port",
+    parser.add_argument("--oscsendport",
         type=int, default=5005, help="The port to connect to")
     
 
@@ -144,14 +155,12 @@ if __name__ == "__main__":
 
         oscreceive = OSCReceive(args.listenip, args.listenport,ur)    
 
-        oscsend.setupReceivers([args.clientip],args.port)
+        oscsend.setupReceivers([args.oscsendip],args.oscsendport)
 
         
 
-    
     curses.wrapper(main)
 
-
     if oscreceive is not None:
         oscreceive.disconnect()
     if ur is not None: