From fe9cf333f9704c61fc8088e46919e9d82ffd345f Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Thu, 9 May 2024 19:17:11 +0200 Subject: [PATCH] add osc receive and send functions --- urosc.py | 93 +++++++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 76 insertions(+), 17 deletions(-) diff --git a/urosc.py b/urosc.py index 74f0053..09d173a 100644 --- a/urosc.py +++ b/urosc.py @@ -8,7 +8,8 @@ import math import numpy as np import curses -from rtde_receive import RTDEReceiveInterface as RTDEReceive +import rtde_receive +import rtde_control @@ -16,13 +17,30 @@ from rtde_receive import RTDEReceiveInterface as RTDEReceive clients = [] rtde_r=None +rtde_c=None server=None #Looptimings frequency_loop=500 frequency_oscsend=50 frequency_rtde=None -frequency_scr=30 +frequency_scr=25 + +#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 toRad(d): return d/360*2*math.pi @@ -35,15 +53,25 @@ def mapFromTo(x,a,b,c,d): y=(x-a)/(b-a)*(d-c)+c return y -def handle_received_osc(address, *args): +def constrain(v,_min,_max): + return min(_max,max(_min,v)) + +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 arg in args: - print(f"Data: {arg}") + 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("/filter", handle_received_osc) # You can change '/filter' to any address pattern you expect + 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) @@ -67,7 +95,9 @@ def setupClients(clientips,port): def setupRTDE(robotip,frequency): dt = 1 / frequency - rtde_r = RTDEReceive(robotip, frequency) + rtde_r = rtde_receive.RTDEReceiveInterface(robotip, frequency) + + rtde_c = rtde_control.RTDEControlInterface(robotip) return rtde_r @@ -109,19 +139,21 @@ def main(stdscr): last_sendosc=0 last_receivertde=0 + last_sendrtde=0 last_updatescr=0 q=[0,0,0,0,0,0] + try: while key != ord('q'): - looptime=time.time() + looptime = time.time() start = time.time() #Get robot joint angled - if looptime-last_receivertde>=1.0/frequency_rtde: + if looptime-last_receivertde>=1.0/frequency_receivertde: last_receivertde=looptime if rtde_r is not None: @@ -141,6 +173,15 @@ def main(stdscr): for jointi in range(6): c.send_message("/j"+str(jointi), toDeg(q[jointi])) + 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) + #Display if looptime-last_updatescr>=1.0/frequency_scr: @@ -148,8 +189,9 @@ def main(stdscr): key = stdscr.getch() stdscr.clear() - stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(3)) + stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(1)) stdscr.addstr(1, 0, "loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)), curses.color_pair(1)) + stdscr.addstr(2, 0, "osc receive frequency="+str(round(1.0/np.mean(oscreceiveintervals),0))+" \t pos="+str(oscreceiveintervals_pos), curses.color_pair(1)) startbarsx=10 endbarsx=width-1 @@ -170,11 +212,11 @@ def main(stdscr): poschar="]" if (len(poschar)>0): - stdscr.addstr(2+j, px, poschar, curses.color_pair(1)) + stdscr.addstr(3+j, px, poschar, curses.color_pair(1)) valuetext=str(round(toDeg(q[j]),2)) if q[j]>=0: #has no negative sign valuetext=" "+valuetext - stdscr.addstr(2+j, 0, valuetext, curses.color_pair(1)) + stdscr.addstr(3+j, 0, valuetext, curses.color_pair(1)) # Refresh the screen @@ -187,6 +229,9 @@ def main(stdscr): loopdurations_pos+=1 loopdurations_pos%=np.size(loopdurations) + + + if loopduration < loop_dt: time.sleep(loop_dt - loopduration) loopcounter += 1 @@ -217,16 +262,27 @@ if __name__ == "__main__": parser.add_argument( - "-f", - "--frequency", - dest="frequency", - help="the frequency at which the data is recorded (default is 500Hz)", + "-fs", + "--frequencysend", + dest="frequencysend", + help="the frequency at which the robot control data is send", type=float, default=500.0, metavar="") + + parser.add_argument( + "-fr", + "--frequencyreceive", + dest="frequencyreceive", + help="the frequency at which the robot actualq data is received", + type=float, + default=500.0, + metavar="") + args = parser.parse_args() - frequency_rtde=args.frequency + frequency_receivertde=args.frequencyreceive + frequency_sendrtde=args.frequencysend clientips=[args.clientip] @@ -245,4 +301,7 @@ if __name__ == "__main__": input("Ended. Press Enter to shutdown") server.shutdown() server_thread.join() + if rtde_c is not None: + rtde_c.servoStop() + rtde_c.stopScript()