add looptiming
This commit is contained in:
parent
14b41f0f96
commit
0fc974b6d3
1 changed files with 75 additions and 60 deletions
135
urosc.py
135
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="<frequency>")
|
||||
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)
|
||||
|
|
|
|||
Loading…
Reference in a new issue