import rtde_control import rtde_receive import time import math import socket import numpy as np import pygame use_rtde=False pygame.init() width=500 height=500 screen = pygame.display.set_mode([width, height]) 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 ="" port =30002 s.bind((host,port)) s.listen(1) # Number of connections s.setblocking(False) client = None # Robot Control Stuff if use_rtde: rtde_c = rtde_control.RTDEControlInterface("192.168.1.101") rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.101") def toRad(d): return d/360*2*math.pi def toDeg(r): return r*360/2/math.pi def RPYtoVec(roll,pitch,yaw): yawMatrix = np.matrix([ [math.cos(yaw), -math.sin(yaw), 0], [math.sin(yaw), math.cos(yaw), 0], [0, 0, 1] ]) pitchMatrix = np.matrix([ [math.cos(pitch), 0, math.sin(pitch)], [0, 1, 0], [-math.sin(pitch), 0, math.cos(pitch)] ]) rollMatrix = np.matrix([ [1, 0, 0], [0, math.cos(roll), -math.sin(roll)], [0, math.sin(roll), math.cos(roll)] ]) R = yawMatrix * pitchMatrix * rollMatrix theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1) / 2) multi = 1 / (2 * math.sin(theta)) rx = multi * (R[2, 1] - R[1, 2]) * theta ry = multi * (R[0, 2] - R[2, 0]) * theta rz = multi * (R[1, 0] - R[0, 1]) * theta return rx, ry, rz paused=True if use_rtde: actual_q = rtde_r.getActualQ() #current joint rotations in radians. print("Actual Q:"+str(actual_q)) #fw_actual_q = rtde_c.getForwardKinematics(actual_q) fw_actual_q = rtde_c.getForwardKinematics() print("forward Actual Q:"+str(fw_actual_q)) joint_q=rtde_c.getInverseKinematics(fw_actual_q) print("Final Q:"+str(joint_q)) winkellimit=45 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 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 #rtde_c.moveJ([0,toRad(-90),toRad(90-tilt/2),toRad(-180-tilt/2),toRad(-90-pan),toRad(0)],0.1,0.1) if use_rtde: rtde_c.moveJ(joint_q,0.2,0.1) #move to initial position # Parameters velocity = 0.2 #0.5 acceleration = 0.1 #0.5 frequency= 100 dt = 1.0/frequency # 2ms lookahead_time = 0.1 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=speed_1 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)] def constrain(v,_min,_max): return min(_max,max(_min,v)) print("Press q to stop") print("Press p to pause") print("Press 1,2,3.. for Setting") print("Press s to resume") print("Starting Servo") 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.key == pygame.K_1: speed=speed_1 if event.key == pygame.K_2: speed=speed_2 if event.key == pygame.K_3: speed=speed_3 if event.type == pygame.QUIT: running = False # Receive Socket stuff try: client, address = s.accept() print("Connected to", address) except socket.error: pass if client is not None: try: data = client.recv( 1024 ).decode( 'utf-8' ) if data: lastdata=data.split('\n') #print("Received :", repr(data)) if (len(lastdata)>1): splitdata=lastdata[-2].split(",") if len(splitdata)==2: try: _pan=float(splitdata[0]) _tilt=float(splitdata[1]) 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") except socket.error: pass # Robot Control stuff if use_rtde: 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 # rz+ = rotate CW reachable=rtde_c.isPoseWithinSafetyLimits(pos_q) if not reachable: print("Reachable:"+str(reachable)) insideJointConstraints=True 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 #print(joint_q) 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))+"°") 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))+"°") 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,"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 s to resume",250,height-20-20*0,20,c_text) pygame.display.flip() end = time.time() stepduration = end - start if stepduration < dt: time.sleep(dt - stepduration) print("Finished") if use_rtde: rtde_c.servoStop() rtde_c.stopScript() s.close()