diff --git a/urosc/oscreceive.py b/urosc/oscreceive.py new file mode 100644 index 0000000..884a656 --- /dev/null +++ b/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() diff --git a/urosc/oscsend.py b/urosc/oscsend.py index a764cf2..64ffd1a 100644 --- a/urosc/oscsend.py +++ b/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)) \ No newline at end of file + c.send_message("/j"+str(jointi), toDeg(joint)) diff --git a/urosc/ui.py b/urosc/ui.py index 1f5f976..16f8584 100644 --- a/urosc/ui.py +++ b/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 diff --git a/urosc/ur.py b/urosc/ur.py new file mode 100644 index 0000000..a3f5a82 --- /dev/null +++ b/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() + + diff --git a/urosc/urosc.py b/urosc/urosc.py index 6d89a30..4222e88 100644 --- a/urosc/urosc.py +++ b/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() - - #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] - + ur.receive(looptime) #send data to all osc receivers - oscsend.send(looptime,q) - - 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) + oscsend.send(looptime,ur.getReceivedQ()) + 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="") + ''' 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") - server.shutdown() - server_thread.join() - if rtde_c is not None: - rtde_c.servoStop() - rtde_c.stopScript() + + if oscreceive is not None: + oscreceive.disconnect() + if ur is not None: + ur.disconnect()