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

updates from wissenschaftstag

Browse source
This commit is contained in:
Philipp Kramer 2022-06-14 20:01:12 +02:00
parent 6f533caf5b
commit 33c824279e
1 changed files with 31 additions and 18 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

49
pantilt_inv.py
View file

@ -1,4 +1,4 @@
import rtde_control import rtde_control #pip install ur_rtde (use python 3.9)
import rtde_receive import rtde_receive
import time import time
import math import math
@ -7,7 +7,7 @@ import numpy as np
import pygame import pygame
use_rtde=False use_rtde=True
pygame.init() pygame.init()
width=500 width=500
@ -107,7 +107,8 @@ if use_rtde:
winkellimit=45 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 distfromcenter=0.9
basedeviation=0 basedeviation=0
centerpos=[math.cos(toRad(180))*distfromcenter,math.sin(toRad(180))*distfromcenter,0.91] 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_1=[0.2,0.2,0.2,0.2,0.2,0.2]
speed_2=[1,1,1,3,3,3] speed_2=[0.5,0.5,0.5,1,1,1]
speed_3=[2,2,2,4,4,4] speed_3=[1,1,1,3,3,3]
speed_4=[2,2,2,4,4,4]
speed=speed_1 speed=speed_1
@ -178,6 +180,8 @@ while running:
speed=speed_2 speed=speed_2
if event.key == pygame.K_3: if event.key == pygame.K_3:
speed=speed_3 speed=speed_3
if event.key == pygame.K_4:
speed=speed_4
if event.type == pygame.QUIT: if event.type == pygame.QUIT:
running = False running = False
@ -213,7 +217,7 @@ while running:
pan=_pan pan=_pan
tilt=_tilt tilt=_tilt
print("Pan="+str(pan)+" Tilt="+str(tilt)) #print("Pan="+str(pan)+" Tilt="+str(tilt))
except ValueError: except ValueError:
print("Not a float") print("Not a float")
except socket.error: except socket.error:
@ -223,8 +227,8 @@ while running:
# Robot Control stuff # Robot Control stuff
if use_rtde: if use_rtde:
if not paused: if not paused:
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation+pan*1/frequency)) basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation+pan*basespeed/frequency))
basedeviation*=-winkellimit*1/frequency/maxbasedeviation+1 basedeviation*=-winkellimit*basespeed/frequency/maxbasedeviation+1
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation)) basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation))
@ -268,28 +272,37 @@ while running:
screen.fill((0,0,0)) screen.fill((0,0,0))
#pygame.draw.circle(screen, (0, 0, 255), (250, 250), 75) #pygame.draw.circle(screen, (0, 0, 255), (250, 250), 75)
jointnames=["Base","Shoulder","Elbow","Wrist1","Wrist2","Wrist3"] 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): 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,(10,10+30*10),(100,20),mapFromTo(basedeviation,-maxbasedeviation,maxbasedeviation,0.0,1.0),"basedeviation",str(round(basedeviation,2))+"°")
drawBar(screen,(200,10+30*1),(100,20),mapFromTo(tilt,-winkellimit,winkellimit,0.0,1.0),"Tilt",str(round(tilt,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,"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,"speed: "+str(speed),10,height-20-12*4-20,20,c_text)
text_to_screen(screen,"1: "+str(speed_1),10,height-20-12*2,12,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*1,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*0,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 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 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) text_to_screen(screen,"Press s to resume",250,height-20-20*0,20,c_text)