philipp.kramer/urcontrol
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

add looptiming

Browse source
This commit is contained in:
Philipp Kramer 2024-05-09 18:46:24 +02:00
parent 14b41f0f96
commit 0fc974b6d3
1 changed files with 75 additions and 60 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

61
urosc.py
View file

@ -18,7 +18,11 @@ clients = []
rtde_r=None rtde_r=None
server=None server=None
#Looptimings
frequency_loop=500
frequency_oscsend=50
frequency_rtde=None
frequency_scr=30
def toRad(d): def toRad(d):
return d/360*2*math.pi 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): 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 # Clear and refresh the screen for a blank canvas
stdscr.clear() stdscr.clear()
stdscr.refresh() stdscr.refresh()
@ -99,9 +97,8 @@ def main(stdscr):
height, width = stdscr.getmaxyx() height, width = stdscr.getmaxyx()
scrfrequency=30
dt = 1 / scrfrequency loop_dt = 1 / frequency_loop
@ -110,13 +107,23 @@ def main(stdscr):
loopdurations = np.zeros(100) loopdurations = np.zeros(100)
loopdurations_pos=0 loopdurations_pos=0
try: last_sendosc=0
while key != ord('q'): last_receivertde=0
key = stdscr.getch() last_updatescr=0
stdscr.clear()
start = time.time()
q=[0,0,0,0,0,0] q=[0,0,0,0,0,0]
try:
while key != ord('q'):
looptime=time.time()
start = time.time()
#Get robot joint angled
if looptime-last_receivertde>=1.0/frequency_rtde:
last_receivertde=looptime
if rtde_r is not None: if rtde_r is not None:
q=rtde_r.getActualQ() q=rtde_r.getActualQ()
else: else:
@ -127,16 +134,22 @@ def main(stdscr):
q[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(joint_max[_i]-joint_min[_i])+joint_min[_i] q[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(joint_max[_i]-joint_min[_i])+joint_min[_i]
#send data to all osc clients
if looptime-last_sendosc>=1.0/frequency_oscsend:
last_sendosc=looptime
for c in clients: for c in clients:
for jointi in range(6): for jointi in range(6):
c.send_message("/j"+str(jointi), toDeg(q[jointi])) 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))
#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))
startbarsx=10 startbarsx=10
endbarsx=width-1 endbarsx=width-1
@ -167,20 +180,20 @@ def main(stdscr):
# Refresh the screen # Refresh the screen
stdscr.refresh() stdscr.refresh()
#Looptiming
end = time.time() end = time.time()
loopduration = end - start loopduration = end - start
loopdurations[loopdurations_pos]=loopduration loopdurations[loopdurations_pos]=loopduration
loopdurations_pos+=1 loopdurations_pos+=1
loopdurations_pos%=np.size(loopdurations) loopdurations_pos%=np.size(loopdurations)
if loopduration < dt: if loopduration < loop_dt:
time.sleep(dt - loopduration) time.sleep(loop_dt - loopduration)
loopcounter += 1 loopcounter += 1
except KeyboardInterrupt: except KeyboardInterrupt:
print("\nData recording stopped.") print("\nKeyboardInterrupt")
curses.endwin() curses.endwin()
@ -213,6 +226,8 @@ if __name__ == "__main__":
metavar="<frequency>") metavar="<frequency>")
args = parser.parse_args() args = parser.parse_args()
frequency_rtde=args.frequency
clientips=[args.clientip] clientips=[args.clientip]
@ -221,7 +236,7 @@ if __name__ == "__main__":
clients=setupClients(clientips,args.port) clients=setupClients(clientips,args.port)
rtde_r=setupRTDE(args.robotip,args.frequency) rtde_r=setupRTDE(args.robotip,frequency_rtde)
curses.wrapper(main) curses.wrapper(main)