updates from wissenschaftstag
This commit is contained in:
parent
6f533caf5b
commit
33c824279e
1 changed files with 31 additions and 18 deletions
|
|
@ -1,4 +1,4 @@
|
|||
import rtde_control
|
||||
import rtde_control #pip install ur_rtde (use python 3.9)
|
||||
import rtde_receive
|
||||
import time
|
||||
import math
|
||||
|
|
@ -7,7 +7,7 @@ import numpy as np
|
|||
|
||||
import pygame
|
||||
|
||||
use_rtde=False
|
||||
use_rtde=True
|
||||
|
||||
pygame.init()
|
||||
width=500
|
||||
|
|
@ -107,7 +107,8 @@ if use_rtde:
|
|||
|
||||
winkellimit=45
|
||||
|
||||
maxbasedeviation=20 # in degrees, maximum rotation deviation from 0°
|
||||
maxbasedeviation=60 # in degrees, maximum rotation deviation from 0°
|
||||
basespeed=1.5
|
||||
distfromcenter=0.9
|
||||
basedeviation=0
|
||||
centerpos=[math.cos(toRad(180))*distfromcenter,math.sin(toRad(180))*distfromcenter,0.91]
|
||||
|
|
@ -138,8 +139,9 @@ gain = 300
|
|||
|
||||
|
||||
speed_1=[0.2,0.2,0.2,0.2,0.2,0.2]
|
||||
speed_2=[1,1,1,3,3,3]
|
||||
speed_3=[2,2,2,4,4,4]
|
||||
speed_2=[0.5,0.5,0.5,1,1,1]
|
||||
speed_3=[1,1,1,3,3,3]
|
||||
speed_4=[2,2,2,4,4,4]
|
||||
|
||||
speed=speed_1
|
||||
|
||||
|
|
@ -178,6 +180,8 @@ while running:
|
|||
speed=speed_2
|
||||
if event.key == pygame.K_3:
|
||||
speed=speed_3
|
||||
if event.key == pygame.K_4:
|
||||
speed=speed_4
|
||||
|
||||
if event.type == pygame.QUIT:
|
||||
running = False
|
||||
|
|
@ -213,7 +217,7 @@ while running:
|
|||
pan=_pan
|
||||
tilt=_tilt
|
||||
|
||||
print("Pan="+str(pan)+" Tilt="+str(tilt))
|
||||
#print("Pan="+str(pan)+" Tilt="+str(tilt))
|
||||
except ValueError:
|
||||
print("Not a float")
|
||||
except socket.error:
|
||||
|
|
@ -223,8 +227,8 @@ while running:
|
|||
# Robot Control stuff
|
||||
if use_rtde:
|
||||
if not paused:
|
||||
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation+pan*1/frequency))
|
||||
basedeviation*=-winkellimit*1/frequency/maxbasedeviation+1
|
||||
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation+pan*basespeed/frequency))
|
||||
basedeviation*=-winkellimit*basespeed/frequency/maxbasedeviation+1
|
||||
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation))
|
||||
|
||||
|
||||
|
|
@ -268,28 +272,37 @@ while running:
|
|||
screen.fill((0,0,0))
|
||||
#pygame.draw.circle(screen, (0, 0, 255), (250, 250), 75)
|
||||
jointnames=["Base","Shoulder","Elbow","Wrist1","Wrist2","Wrist3"]
|
||||
|
||||
|
||||
drawBar(screen,(10,10+30*0),(100,20),mapFromTo(pan,-winkellimit,winkellimit,0.0,1.0),"Pan",str(round(pan,2))+"°")
|
||||
drawBar(screen,(10,10+30*1),(100,20),mapFromTo(tilt,-winkellimit,winkellimit,0.0,1.0),"Tilt",str(round(tilt,2))+"°")
|
||||
|
||||
|
||||
for i in range(6):
|
||||
drawBar(screen,(10,10+30*i),(100,20),mapFromTo(joint_q[i],joint_min[i],joint_max[i],0.0,1.0),jointnames[i],str(round(toDeg(joint_q[i]),2))+"°")
|
||||
drawBar(screen,(10,10+30*(3+i)),(100,20),mapFromTo(joint_q[i],joint_min[i],joint_max[i],0.0,1.0),jointnames[i],str(round(toDeg(joint_q[i]),2))+"°")
|
||||
|
||||
for i in range(6):
|
||||
_jointdiff = joint_q_aim[i]-joint_q[i]
|
||||
drawBar(screen,(200,10+30*(3+i)),(100,20),mapFromTo(_jointdiff,-speed[i]/frequency,speed[i]/frequency,0.0,1.0),"diff_"+str(jointnames[i]),str(round(toDeg(_jointdiff),3))+"°")
|
||||
|
||||
|
||||
drawBar(screen,(10,10+30*8),(100,20),mapFromTo(basedeviation,-maxbasedeviation,maxbasedeviation,0.0,1.0),"basedeviation",str(round(basedeviation,2))+"°")
|
||||
|
||||
drawBar(screen,(200,10+30*0),(100,20),mapFromTo(pan,-winkellimit,winkellimit,0.0,1.0),"Pan",str(round(pan,2))+"°")
|
||||
drawBar(screen,(200,10+30*1),(100,20),mapFromTo(tilt,-winkellimit,winkellimit,0.0,1.0),"Tilt",str(round(tilt,2))+"°")
|
||||
drawBar(screen,(10,10+30*10),(100,20),mapFromTo(basedeviation,-maxbasedeviation,maxbasedeviation,0.0,1.0),"basedeviation",str(round(basedeviation,2))+"°")
|
||||
|
||||
drawBar(screen,(200,10+30*5),(100,20),mapFromTo(np.mean(receivetimearray),0,1.0,0.0,1.0),"Receive Interval",str(round(np.mean(receivetimearray),2))+"s")
|
||||
drawBar(screen,(200,10+30*10),(100,20),mapFromTo(np.mean(receivetimearray),0,1.0,0.0,1.0),"Receive Interval",str(round(np.mean(receivetimearray),3))+"s")
|
||||
drawBar(screen,(200,10+30*11),(100,20),mapFromTo(np.max(receivetimearray),0,1.0,0.0,1.0),"Max Receive Int.",str(round(np.max(receivetimearray),3))+"s")
|
||||
|
||||
|
||||
text_to_screen(screen,"paused: "+str(paused),10,height-20-12*2-20*3,20,c_text)
|
||||
text_to_screen(screen,"speed: "+str(speed),10,height-20-12*2-20,20,c_text)
|
||||
text_to_screen(screen,"1: "+str(speed_1),10,height-20-12*2,12,c_text)
|
||||
text_to_screen(screen,"2: "+str(speed_2),10,height-20-12*1,12,c_text)
|
||||
text_to_screen(screen,"3: "+str(speed_3),10,height-20-12*0,12,c_text)
|
||||
text_to_screen(screen,"speed: "+str(speed),10,height-20-12*4-20,20,c_text)
|
||||
text_to_screen(screen,"1: "+str(speed_1),10,height-20-12*3,12,c_text)
|
||||
text_to_screen(screen,"2: "+str(speed_2),10,height-20-12*2,12,c_text)
|
||||
text_to_screen(screen,"3: "+str(speed_3),10,height-20-12*1,12,c_text)
|
||||
text_to_screen(screen,"4: "+str(speed_4),10,height-20-12*0,12,c_text)
|
||||
|
||||
text_to_screen(screen,"Press q to stop",250,height-20-20*3,20,c_text)
|
||||
text_to_screen(screen,"Press p to pause",250,height-20-20*2,20,c_text)
|
||||
text_to_screen(screen,"Press 1,2,3 to change speed",250,height-20-20*1,20,c_text)
|
||||
text_to_screen(screen,"Press 1,2,3,4 to change speed",250,height-20-20*1,20,c_text)
|
||||
text_to_screen(screen,"Press s to resume",250,height-20-20*0,20,c_text)
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue