add base rotation

tests/pantilt_inv.py

@@ -10,6 +10,32 @@ pygame.init()
 screen = pygame.display.set_mode([500, 500])
 running = True
 
+c_border = (100,100,100)
+c_name=(200,200,200)
+c_cursor = (255,255,255)
+c_text=(255,255,255)
+
+def mapFromTo(x,a,b,c,d):
+   y=(x-a)/(b-a)*(d-c)+c
+   return y
+
+def text_to_screen(screen, text, x, y, size = 50,
+    color = (200, 000, 000)):
+
+    text = str(text)
+    font = pygame.font.SysFont(None, size)
+    text = font.render(text, True, color)
+    screen.blit(text, (x, y))
+
+    
+def drawBar(screen,_topleft,_size,_pos,_name,_text):
+    pygame.draw.rect(screen, c_border, (_topleft[0],_topleft[1],_size[0],_size[1]))
+    text_to_screen(screen,_name,_topleft[0],_topleft[1],_size[1],c_name)
+    pygame.draw.line(screen, c_cursor, (_topleft[0]+_pos*_size[0],_topleft[1]),(_topleft[0]+_pos*_size[0],_topleft[1]+_size[1]))
+    text_to_screen(screen,_text,_size[0]+_topleft[0],_topleft[1],_size[1],c_text)
+
+
+
 # Socket stuff
 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
 host =""
@@ -74,8 +100,13 @@ print("forward Actual Q:"+str(fw_actual_q))
 joint_q=rtde_c.getInverseKinematics(fw_actual_q)
 print("Final Q:"+str(joint_q))
 
+winkellimit=45
 
-centerpos=[-0.895,-0.177,0.91]
+maxbasedeviation=20 # in degrees, maximum rotation deviation from 0°
+distfromcenter=0.9
+basedeviation=0
+centerpos=[math.cos(toRad(180))*distfromcenter,math.sin(toRad(180))*distfromcenter,0.91]
+#centerpos=[-0.895,-0.177,0.91]
 centerrotRPY=[-1.572,0,1.572] #roll, pitch, yaw
 
 pan=0 #-=left, +=right, degrees
@@ -84,13 +115,6 @@ tilt=0 #-=down, +=up, degrees
 joint_q_init=[0,toRad(-90),toRad(90),toRad(-180),toRad(-90),toRad(0)]   #start position
 joint_q=joint_q_init
 
-'''
-_rot=RPYtoVec(centerrotRPY[0],centerrotRPY[1],centerrotRPY[2])
-pos_q=[centerpos[0],centerpos[1],centerpos[2],_rot[0],_rot[1],_rot[2]]
-
-joint_q2=rtde_c.getInverseKinematics(pos_q,joint_q_init)
-print("Ninal Q:"+str(joint_q2))
-'''
 
 #rtde_c.moveJ([0,toRad(-90),toRad(90-tilt/2),toRad(-180-tilt/2),toRad(-90-pan),toRad(0)],0.1,0.1)
 
@@ -162,7 +186,7 @@ while running:
                             _pan=float(splitdata[0])
                             _tilt=float(splitdata[1])
 
-                            winkellimit=45
+                            
                             if _pan>=-winkellimit and _pan <=winkellimit and _tilt>=-winkellimit and _tilt <=winkellimit:
                                 pan=_pan
                                 tilt=_tilt
@@ -175,9 +199,15 @@ while running:
 
 
     # Robot Control stuff
+    if not paused:
+        basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation+pan*1/frequency))
+
+    baserotation=toRad(180-basedeviation) #positive is ccw
+    centerpos=[math.cos(baserotation)*distfromcenter,math.sin(baserotation)*distfromcenter,0.91]
     _rot=RPYtoVec(centerrotRPY[0]+toRad(tilt),centerrotRPY[1],centerrotRPY[2]+toRad(-pan))
     pos_q=[centerpos[0],centerpos[1],centerpos[2],_rot[0],_rot[1],_rot[2]]
     joint_q_aim=rtde_c.getInverseKinematics(pos_q,joint_q_init)
+
     #joint_q_aim = rtde_c.getInverseKinematics([-0.895,-0.173,0.911,2.4,2.4,2.4])
     # inverse kinematics: rx+ =look up
     # ry+ = look right
@@ -187,41 +217,39 @@ while running:
     if not reachable:
         print("Reachable:"+str(reachable))
 
+
     insideJointConstraints=True
-    if joint_q_aim[0]<toRad(-20) or joint_q_aim[0]>toRad(20):
+    joint_min=[toRad(-90),toRad(-90-40),toRad(90-90),toRad(-180-80),toRad(-90-90),toRad(0-75)]
-        print("Joint 0 Outside constraint with "+str(joint_q_aim[0])+" ("+str(toDeg(joint_q_aim[0]))+")")
+    joint_max=[toRad(90),toRad(-90+40),toRad(90+40),toRad(-180+110),toRad(-90+90),toRad(0+75)]
-        insideJointConstraints=False
+    for i in range(6):
-    if joint_q_aim[1]<toRad(-90-40) or joint_q_aim[1]>toRad(-90+40):
+        if joint_q_aim[i]<joint_min[i] or joint_q_aim[i]>joint_max[i]:
-        print("Joint 1 Outside constraint with "+str(joint_q_aim[1])+" ("+str(toDeg(joint_q_aim[1]))+")")
+            print("Joint "+str(i)+" Outside constraint with "+str(joint_q_aim[i])+" ("+str(toDeg(joint_q_aim[i]))+")")
-        insideJointConstraints=False
+            insideJointConstraints=False
-    if joint_q_aim[2]<toRad(90-90) or joint_q_aim[2]>toRad(90+40):
-        print("Joint 2 Outside constraint with "+str(joint_q_aim[2])+" ("+str(toDeg(joint_q_aim[2]))+")")
-        insideJointConstraints=False
-    if joint_q_aim[3]<toRad(-180-80) or joint_q_aim[3]>toRad(-180+110):
-        print("Joint 3 Outside constraint with "+str(joint_q_aim[3])+" ("+str(toDeg(joint_q_aim[3]))+")")
-        insideJointConstraints=False
-    if joint_q_aim[4]<toRad(-90-70) or joint_q_aim[4]>toRad(-90+70):
-        print("Joint 4 Outside constraint with "+str(joint_q_aim[4])+" ("+str(toDeg(joint_q_aim[4]))+")")
-        insideJointConstraints=False
-    if joint_q_aim[5]<toRad(0-60) or joint_q_aim[5]>toRad(0+60):
-        print("Joint 5 Outside constraint with "+str(joint_q_aim[5])+" ("+str(toDeg(joint_q_aim[5]))+")")
-        insideJointConstraints=False
 
 
     if reachable and insideJointConstraints and not paused:
         for i in range(6):
             joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed[i]/frequency,speed[i]/frequency) #constrain joint speeds
         
-
-
     
     #print(joint_q)
 
     if not paused:
         rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain)
 
-    screen.fill((255, 255, 255))
+    
-    pygame.draw.circle(screen, (0, 0, 255), (250, 250), 75)
+
+    screen.fill((0,0,0))
+    #pygame.draw.circle(screen, (0, 0, 255), (250, 250), 75)
+    jointnames=["Base","Shoulder","Elbow","Wrist1","Wrist2","Wrist3"]
+    
+    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*8),(100,20),mapFromTo(basedeviation,-maxbasedeviation,maxbasedeviation,0.0,1.0),"basedeviation",str(round(basedeviation,2))+"°")
+        
+    
     pygame.display.flip()
     
     end = time.time()
@@ -241,4 +269,3 @@ rtde_c.stopScript()
 
 s.close()
 
-