diff --git a/tests/pantilt_inv.py b/tests/pantilt_inv.py index 91b7161..69b913a 100644 --- a/tests/pantilt_inv.py +++ b/tests/pantilt_inv.py @@ -2,9 +2,40 @@ import rtde_control import time import math import socket -import keyboard # pip install keyboard import numpy as np +import pygame + +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 ="" @@ -59,7 +90,7 @@ def RPYtoVec(roll,pitch,yaw): return rx, ry, rz -paused=False +paused=True actual_q = rtde_r.getActualQ() #current joint rotations in radians. print("Actual Q:"+str(actual_q)) @@ -69,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 @@ -79,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) @@ -109,19 +138,33 @@ speed=[1,1,1,3,3,3] def constrain(v,_min,_max): return min(_max,max(_min,v)) -print("Press s to start") + print("Press q to stop") print("Press p to pause") -while not keyboard.is_pressed("s"): - if keyboard.is_pressed("q"): - exit() - time.sleep(0.1) +print("Press s to resume") + print("Starting Servo") -while not keyboard.is_pressed("q"): +while running: start = time.time() + + + for event in pygame.event.get(): + if event.type == pygame.KEYDOWN: + if event.key == pygame.K_p and paused==False: + print("paused") + paused=True + if event.key == pygame.K_s and paused==True: + print("resumed") + paused=False + if event.key == pygame.K_q: + print("stop") + running=False + if event.type == pygame.QUIT: + running = False + # Receive Socket stuff try: client, address = s.accept() @@ -138,27 +181,33 @@ while not keyboard.is_pressed("q"): #print("Received :", repr(data)) if (len(lastdata)>1): splitdata=lastdata[-2].split(",") - - try: - _pan=float(splitdata[0]) - _tilt=float(splitdata[1]) + if len(splitdata)==2: + try: + _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 + + if _pan>=-winkellimit and _pan <=winkellimit and _tilt>=-winkellimit and _tilt <=winkellimit: + pan=_pan + tilt=_tilt - print("Pan="+str(pan)+" Tilt="+str(tilt)) - except ValueError: - print("Not a float") + print("Pan="+str(pan)+" Tilt="+str(tilt)) + except ValueError: + print("Not a float") except socket.error: pass # 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 @@ -168,43 +217,40 @@ while not keyboard.is_pressed("q"): if not reachable: print("Reachable:"+str(reachable)) + insideJointConstraints=True - if joint_q_aim[0]toRad(20): - print("Joint 0 Outside constraint with "+str(joint_q_aim[0])+" ("+str(toDeg(joint_q_aim[0]))+")") - insideJointConstraints=False - if joint_q_aim[1]toRad(-90+40): - print("Joint 1 Outside constraint with "+str(joint_q_aim[1])+" ("+str(toDeg(joint_q_aim[1]))+")") - insideJointConstraints=False - if 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+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): - 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): - print("Joint 5 Outside constraint with "+str(joint_q_aim[5])+" ("+str(toDeg(joint_q_aim[5]))+")") - insideJointConstraints=False + joint_min=[toRad(-90),toRad(-90-40),toRad(90-90),toRad(-180-80),toRad(-90-90),toRad(0-75)] + joint_max=[toRad(90),toRad(-90+40),toRad(90+40),toRad(-180+110),toRad(-90+90),toRad(0+75)] + for i in range(6): + if joint_q_aim[i]joint_max[i]: + print("Joint "+str(i)+" Outside constraint with "+str(joint_q_aim[i])+" ("+str(toDeg(joint_q_aim[i]))+")") + 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 - - - if keyboard.is_pressed("p"): - print("paused") - paused=True - if keyboard.is_pressed("s"): - print("resumed") - paused=False #print(joint_q) - rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain) + if not paused: + rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain) + + + + 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() stepduration = end - start @@ -223,4 +269,3 @@ rtde_c.stopScript() s.close() -