Merge branch 'master' of git.theater.digital:philipp.kramer/urcontrol

pantilt_inv.py

@@ -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)