diff --git a/urosc.py b/urosc.py index 20febba..74f0053 100644 --- a/urosc.py +++ b/urosc.py @@ -18,7 +18,11 @@ clients = [] rtde_r=None server=None - +#Looptimings +frequency_loop=500 +frequency_oscsend=50 +frequency_rtde=None +frequency_scr=30 def toRad(d): return d/360*2*math.pi @@ -75,12 +79,6 @@ joint_max=[toRad(360),toRad(360),toRad(360),toRad(360),toRad(360),toRad(360)] def main(stdscr): - """Main entry point allowing external calls - - Args: - args ([str]): command line parameter list - """ - # Clear and refresh the screen for a blank canvas stdscr.clear() stdscr.refresh() @@ -99,9 +97,8 @@ def main(stdscr): height, width = stdscr.getmaxyx() - scrfrequency=30 - dt = 1 / scrfrequency + loop_dt = 1 / frequency_loop @@ -109,79 +106,95 @@ def main(stdscr): loopcounter = 0 loopdurations = np.zeros(100) loopdurations_pos=0 + + last_sendosc=0 + last_receivertde=0 + last_updatescr=0 + + q=[0,0,0,0,0,0] try: while key != ord('q'): - key = stdscr.getch() - stdscr.clear() + looptime=time.time() + start = time.time() - q=[0,0,0,0,0,0] - 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] + #Get robot joint angled + if looptime-last_receivertde>=1.0/frequency_rtde: + last_receivertde=looptime - for c in clients: - - - for jointi in range(6): - c.send_message("/j"+str(jointi), toDeg(q[jointi])) - - stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(1)) - stdscr.addstr(1, 0, "loop usage="+str(round(np.max(loopdurations)/dt,3)), curses.color_pair(1)) - - - - startbarsx=10 - endbarsx=width-1 - for j in range(6): - posx=mapFromTo(q[j],joint_min[j],joint_max[j],startbarsx,endbarsx) #scale to screen width - for px in range(width): - diff=abs(posx-px) - poschar='' + 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] - if diff<3: - poschar='-' - if diff<0.5: - poschar='|' + 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] - if px==startbarsx: - poschar="[" - if px==endbarsx: - poschar="]" - - if (len(poschar)>0): - stdscr.addstr(2+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)) + #send data to all osc clients + if looptime-last_sendosc>=1.0/frequency_oscsend: + last_sendosc=looptime + for c in clients: + for jointi in range(6): + c.send_message("/j"+str(jointi), toDeg(q[jointi])) + + + #Display + if looptime-last_updatescr>=1.0/frequency_scr: + last_updatescr=looptime + key = stdscr.getch() + stdscr.clear() + + stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(3)) + stdscr.addstr(1, 0, "loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)), curses.color_pair(1)) - # Refresh the screen - stdscr.refresh() + startbarsx=10 + endbarsx=width-1 + for j in range(6): + posx=mapFromTo(q[j],joint_min[j],joint_max[j],startbarsx,endbarsx) #scale to screen width + for px in range(width): + diff=abs(posx-px) + poschar='' + + if diff<3: + poschar='-' + if diff<0.5: + poschar='|' + if px==startbarsx: + poschar="[" + if px==endbarsx: + poschar="]" + + if (len(poschar)>0): + stdscr.addstr(2+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)) + + # Refresh the screen + stdscr.refresh() + + #Looptiming end = time.time() loopduration = end - start loopdurations[loopdurations_pos]=loopduration loopdurations_pos+=1 loopdurations_pos%=np.size(loopdurations) - if loopduration < dt: - time.sleep(dt - loopduration) + if loopduration < loop_dt: + time.sleep(loop_dt - loopduration) loopcounter += 1 except KeyboardInterrupt: - print("\nData recording stopped.") - curses.endwin() + print("\nKeyboardInterrupt") + curses.endwin() if __name__ == "__main__": @@ -213,6 +226,8 @@ if __name__ == "__main__": metavar="") args = parser.parse_args() + frequency_rtde=args.frequency + clientips=[args.clientip] @@ -221,7 +236,7 @@ if __name__ == "__main__": clients=setupClients(clientips,args.port) - rtde_r=setupRTDE(args.robotip,args.frequency) + rtde_r=setupRTDE(args.robotip,frequency_rtde) curses.wrapper(main)