diff --git a/urosc/oscreceive.py b/urosc/oscreceive.py index 884a656..7b23a3e 100644 --- a/urosc/oscreceive.py +++ b/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() - self.server_thread.join() + if self.server is not None: + self.server.shutdown() + if self.server_thread is not None: + self.server_thread.join() diff --git a/urosc/ur.py b/urosc/ur.py index a3f5a82..39a4b5d 100644 --- a/urosc/ur.py +++ b/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.velocitylimit=[1,1,1,3,3,3] #joint velocity limits + 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=[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 - 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.controlEnabled: + 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) + 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): + def receiveJoints(self,looptime): #Get robot joint angles - if looptime-self.last_receivertde>=1.0/self.frequency_rtde_receive: - self.last_receivertde=looptime + if looptime-self.last_receivertde<1.0/self.frequency_rtde_receive: + return + 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] + 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 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() + diff --git a/urosc/urosc.py b/urosc/urosc.py index 4222e88..c01ea04 100644 --- a/urosc/urosc.py +++ b/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: