def grid_place(c, ser_ee, x, y, z, r, XE=0, YE=0): ic.socket_send(c, sCMD=300) # select lego tcp alpha = 0.3 XE = alpha * XE YE = alpha * YE demand_Joints = dict(grid_pos(c, x + XE, y + YE, z + 2, r)) msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2, Speed=1.05) # move above location demand_Joints = dict(grid_pos(c, x, y, z + 0.5, r)) msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2, Speed=0.5) demand_Joints = dict(grid_pos(c, x, y, z, r)) msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2, Speed=0.1) ic.super_serial_send(ser_ee, "G", 51) # open grabber ic.socket_send(c, sCMD=299) demand_Pose = dict(smooth_rotate(c, R=1)) msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=8) # rotate grabber ic.socket_send(c, sCMD=300) #demand_Joints = dict(grid_pos(c,x,y,z+0.3,r)) #msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=0.1) # move back up demand_Joints = dict(grid_pos(c, x, y, z + 2, r)) msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2) # move back up return
def assemble(c,ser_ee,bricks): delay = 0 n = 0 grip = 0 for i in range(0,len(bricks)): # stop after 0,20,40 etc bricks to fill hoppers #if i % 20 == 0: # tic = time.time() # ipt = raw_input("Fill hopper and press enter to continue") # toc = time.time() # delay+=toc-tic # pick brick(s) from feed if none in grip (grip = 0) flag = 1 if bricks[i]['b']==0: # if 2x4 brick use hopper 0 if bricks[i]['p']==3: # if tool placing method, pick 2 bricks if grip == 0: feed_pick(c,ser_ee) feed_pick(c,ser_ee,stack=2) flag = 2 bricks[i]['p']=1 elif (bricks[i]['r'] == 0 or bricks[i]['r'] == 180) and grip == 0: feed_pick(c,ser_ee,X=bricks[i]['p']) elif grip == 0: feed_pick(c,ser_ee,X=2-bricks[i]['p']) elif bricks[i]['b']==1: # if 2x2 brick use hopper 1 if grip == 0: feed_pick(c,ser_ee,X=2,H=1) if bricks[i]['p'] == 3: feed_pick(c,ser_ee,X=2,H=1,stack=2) flag = 2 bricks[i]['p']=0 # home waypoint msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) # grid placing if bricks[i]['b']==0: if bricks[i]['r'] == 0 or bricks[i]['r'] == 180: grid_place(c,ser_ee,bricks[i]['x'],bricks[i]['y']+bricks[i]['p'],bricks[i]['z'],bricks[i]['r'],XE=-bricks[i]['ye'],YE=bricks[i]['xe'],stack=flag) else: grid_place(c,ser_ee,bricks[i]['x']+bricks[i]['p'],bricks[i]['y'],bricks[i]['z'],bricks[i]['r'],XE=bricks[i]['xe'],YE=bricks[i]['ye'],stack=flag) elif bricks[i]['b']==1: grid_place(c,ser_ee,bricks[i]['x'],bricks[i]['y'],bricks[i]['z'],bricks[i]['r'],XE=bricks[i]['xe'],YE=bricks[i]['ye'],stack=flag) grip = 0 # stowing excess bricks if i < len(bricks)-1 and flag == 2: if bricks[i]['b'] == bricks[i+1]['b'] and (bricks[i+1]['p'] == 1 or bricks[i+1]['p'] == 3): # if tool placing, don't stow brick if next place can use it grip = 1 if grip == 0 and flag == 2: msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) feed_place(c,ser_ee,H=bricks[i]['b']) # home waypoint msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) return delay
def disassemble(c,ser_ee,bricks): delay = 0 grip = 0 for i in range(0,len(bricks)): # pick brick, place in feed system, move up, repeat for whole list # stop after 0,20,40 etc bricks to empty hoppers #if i % 20 == 0: # tic = time.time() # ipt = raw_input("Empty hopper and press enter to continue") # toc = time.time() # delay+=toc-tic # pick brick if using tool disassembly flag = 1 if bricks[i]['p'] == 3 and bricks[i]['b'] == 0: if grip == 0: feed_pick(c,ser_ee) msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) flag = 2 bricks[i]['p'] = 1 elif bricks[i]['p'] == 3 and bricks[i]['b'] == 1: if grip == 0: feed_pick(c,ser_ee,X=2,H=1) msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) flag = 2 bricks[i]['p'] = 0 # pick brick from grid if bricks[i]['r'] == 0 or bricks[i]['r'] == 180: grid_pick(c,ser_ee,bricks[i]['x'],bricks[i]['y']+bricks[i]['p'],bricks[i]['z'],bricks[i]['r'],stack=flag) else: grid_pick(c,ser_ee,bricks[i]['x']+bricks[i]['p'],bricks[i]['y'],bricks[i]['z'],bricks[i]['r'],stack=flag) # home waypoint msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) # stow bricks if flag == 2: feed_place2(c,ser_ee,sH=bricks[i]['b']) grip = 0 # if tool picking next, don't stow brick if i < len(bricks)-1 and (flag == 2 or bricks[i]['p'] == 1): if bricks[i+1]['p'] == 3 and bricks[i]['b'] == bricks[i+1]['b']: grip = 1 # stow excess bricks if grip == 0: feed_place(c,ser_ee,H=bricks[i]['b']) # home waypoint msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) return
def master_pick(c,ser_ee,brick): # pick brick from grid # 2x4 brick if brick['b'] == 0: if brick['p'] == 3: if brick['r'] == 0 or brick['r'] == 180: grid_pick(c,ser_ee,brick['x'],brick['y']+1,brick['z'],brick['r'],stack=2) else: grid_pick(c,ser_ee,brick['x']+1,brick['y'],brick['z'],brick['r'],stack=2) else: if brick['r'] == 0 or brick['r'] == 180: grid_pick(c,ser_ee,brick['x'],brick['y']+brick['p'],brick['z'],brick['r'],stack=1) else: grid_pick(c,ser_ee,brick['x']+brick['p'],brick['y'],brick['z'],brick['r'],stack=1) # 2x2 brick elif brick['b'] == 1: if brick['p'] == 3: grid_pick(c,ser_ee,brick['x'],brick['y'],brick['z'],brick['r'],stack=2) else: grid_pick(c,ser_ee,brick['x'],brick['y'],brick['z'],brick['r'],stack=1) # home waypoint msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) return
def get_tool(c,ser_ee,brick,EE_STATE): if brick['p'] == 3 and EE_STATE == EMPTY: if brick['b'] == 0: feed_pick(c,ser_ee) else: feed_pick(c,ser_ee,X=2,H=1) msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) return
def feed_place(c, ser_ee, H=0): ic.socket_send(c, sCMD=300) # select lego tcp (tool centre point) if H == 0: demand_Joints = dict(wp.Hopper0_stow_wp_joints) demand_Pose = dict(wp.Hopper0_stow) else: demand_Joints = dict(wp.Hopper1_stow_wp_joints) demand_Pose = dict(wp.Hopper1_stow) msg = ic.safe_ur_move(c, Pose=dict(demand_Joints), CMD=2, Speed=1.05) msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=8) ic.super_serial_send(ser_ee, "G", 51) demand_Pose['x'] = demand_Pose['x'] + 3 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4) demand_Pose = dict(smooth_rotate(c, R=15)) msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=8) demand_Pose['x'] = demand_Pose['x'] - 3 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4) demand_Pose['y'] = demand_Pose['y'] - 20 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=8) msg = ic.safe_ur_move(c, Pose=dict(demand_Joints), CMD=2, Speed=1.05) return
def feed_pick(c,ser_ee,X=1,H=0,stack=1): ic.socket_send(c,sCMD=300) # select lego tcp (tool centre point) if H==0: # waypoints for hopper and brick position if X == 0: demand_Pose = copy.deepcopy(wp.Hopper0_feed0) elif X == 1: demand_Pose = copy.deepcopy(wp.Hopper0_feed1) else: demand_Pose = copy.deepcopy(wp.Hopper0_feed2) else: if X == 0: demand_Pose = copy.deepcopy(wp.Hopper1_feed0) elif X == 1: demand_Pose = copy.deepcopy(wp.Hopper1_feed1) else: demand_Pose = copy.deepcopy(wp.Hopper1_feed2) if stack == 1: # normal picking demand_Pose['z'] = demand_Pose['z']+20 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4) demand_Pose['z'] = demand_Pose['z']-20 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) ic.super_serial_send(ser_ee,"G",49) demand_Pose['z'] = demand_Pose['z']+40 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4) elif stack == 2: # run after normal picking to pick a second brick demand_Pose['z'] = demand_Pose['z']+40 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4) demand_Pose['z'] = demand_Pose['z']-40+8 print demand_Pose msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8,Speed=0.05) demand_Pose['y'] = demand_Pose['y']+0.5 demand_Pose['x'] = demand_Pose['x']+0.5 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) demand_Pose['y'] = demand_Pose['y']+0.5 demand_Pose['x'] = demand_Pose['x']+0.5 demand_Pose['z'] = demand_Pose['z']+5 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8,Speed=0.001) demand_Pose['y'] = demand_Pose['y']-1 demand_Pose['x'] = demand_Pose['x']-1 demand_Pose['z'] = demand_Pose['z']+35-8 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8,Speed=0.05) return
def stow_excess(c,ser_ee,input_bricks): bricks = list(input_bricks) STATE = 0 if len(bricks) != 1: # stowing excess bricks if bricks[0]['p']==3: if bricks[0]['b'] == 0 and bricks[1]['b'] == 0 and (bricks[1]['p'] == 1 or bricks[1]['p'] == 3): # if tool placing, don't stow brick if next place can use it STATE = X4 elif bricks[0]['b'] == 1 and bricks[1]['b'] == 1: STATE = X2 else: msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) feed_place(c,ser_ee,H=bricks[0]['b']) else: if bricks[0]['p']==3: feed_place(c,ser_ee,H=bricks[0]['b']) # home waypoint msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) return STATE
def grid_place(c,ser_ee,x,y,z,r,XE=0,YE=0,stack=1): ic.socket_send(c,sCMD=300) # select lego tcp alpha = 0.3 XE = alpha*XE YE = alpha*YE demand_Joints = dict(grid_pos(c,x+XE,y+YE,z+1+stack,r)) msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=1.05) # move above location demand_Joints = dict(grid_pos(c,x,y,z-0.5+stack,r)) msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=0.5) demand_Joints = dict(grid_pos(c,x,y,z-1+stack,r)) msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=0.1) if stack == 1: # if normal placing, release brick ic.super_serial_send(ser_ee,"G",51) # open grabber ic.socket_send(c,sCMD=299) demand_Pose = dict(smooth_rotate(c,R=3)) msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) # rotate grabber ic.socket_send(c,sCMD=300) elif stack == 2: # if tool placing, pick top brick demand_Pose = dict(smooth_rotate(c,R=15)) msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) # rotate grabber demand_Joints = dict(grid_pos(c,x,y,z+1+stack,r)) msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2) # move back up return
def grid_pick(c,ser_ee,x,y,z,r,stack=1): ic.socket_send(c,sCMD=300) # select lego tcp (tool centre point) demand_Joints = dict(grid_pos(c,x,y,z+1+stack,r)) msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=1.05) # move above brick #ic.super_serial_send(ser_ee,"G",51) demand_Joints = dict(grid_pos(c,x,y,z-0.1+stack,r)) msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=0.5) demand_Joints = dict(grid_pos(c,x,y,z-1+stack,r)) msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=0.1) if stack == 1: ic.super_serial_send(ser_ee,"G",49) # close grabber ic.socket_send(c,sCMD=299+stack) demand_Pose = dict(smooth_rotate(c,R=15)) #print "demand_Pose: ",demand_Pose #ipt = raw_input("continue?") msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) # rotate grabber current_Pose = ic.get_ur_position(c,1) demand_Pose = {'x':current_Pose[0],'y':current_Pose[1],'z':current_Pose[2]+20,'rx':current_Pose[3],'ry':current_Pose[4],'rz':current_Pose[5]} msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) demand_Joints = dict(grid_pos(c,x,y,z+1+stack,r)) msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=0.5) # move back up
def feed_place2(c,ser_ee,sH=0): ic.socket_send(c,sCMD=300) # select lego tcp (tool centre point) # separate stack by placing in pick position demand_Pose = copy.deepcopy(wp.Hopper0_feed1) demand_Pose['z'] = demand_Pose['z']+38 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4) # move above brick demand_Pose['z'] = demand_Pose['z']-20.5 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8,Speed=0.25) demand_Pose = dict(smooth_rotate(c,R=15)) msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) # rotate grabber current_Pose = ic.get_ur_position(c,1) demand_Pose = {'x':current_Pose[0],'y':current_Pose[1],'z':current_Pose[2]+80,'rx':current_Pose[3],'ry':current_Pose[4],'rz':current_Pose[5]} msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) feed_place(c,ser_ee,H=sH) demand_Pose = copy.deepcopy(wp.Hopper0_feed1) demand_Pose['z'] = demand_Pose['z']+28.5 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4) # move above brick demand_Pose['z'] = demand_Pose['z']-20 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) ic.super_serial_send(ser_ee,"G",49) demand_Pose['x'] = wp.Hopper0_feed0['x'] demand_Pose['y'] = wp.Hopper0_feed0['y'] msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) demand_Pose = dict(smooth_rotate(c,R=30)) msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) # rotate grabber demand_Pose['z'] = demand_Pose['z']+80 msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=8) #feed_place(c,ser_ee,H=sH) return
def disassemble(c, ser_ee, bricks): for i in range( 0, len(bricks) ): # pick brick, place in feed system, move up, repeat for whole list flag = 1 if bricks[i]['p'] == 3: feed_pick(c, ser_ee) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2, Speed=1.05) flag = 2 bricks[i]['p'] = 1 if bricks[i]['r'] == 0 or bricks[i]['r'] == 180: grid_pick(c, ser_ee, bricks[i]['x'], bricks[i]['y'] + bricks[i]['p'], bricks[i]['z'], bricks[i]['r'], stack=flag) else: grid_pick(c, ser_ee, bricks[i]['x'] + bricks[i]['p'], bricks[i]['y'], bricks[i]['z'], bricks[i]['r'], stack=flag) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2, Speed=1.05) stack_place(c, ser_ee, sH=bricks[i]['b'], stack=flag) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2, Speed=1.05) return
def assemble(c, ser_ee, bricks): delay = 0 n = 0 for i in range( 0, len(bricks) ): # get brick from feed system, place in demand position, move up, repeat for whole list if bricks[i]['b'] == 0: if bricks[i]['r'] == 0 or bricks[i]['r'] == 180: feed_pick(c, ser_ee, X=bricks[i]['p']) else: feed_pick(c, ser_ee, X=2 - bricks[i]['p']) elif bricks[i]['b'] == 1: feed_pick(c, ser_ee, X=2, H=1) if bricks[i]['r'] == 0 or bricks[i]['r'] == 180: grid_place(c, ser_ee, bricks[i]['x'], bricks[i]['y'] + bricks[i]['p'], bricks[i]['z'], bricks[i]['r'], XE=-bricks[i]['ye'], YE=bricks[i]['xe']) else: grid_place(c, ser_ee, bricks[i]['x'] + bricks[i]['p'], bricks[i]['y'], bricks[i]['z'], bricks[i]['r'], XE=bricks[i]['xe'], YE=bricks[i]['ye']) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2, Speed=1.05) n += 1 if n == 25: tic = time.time() ipt = raw_input("Refill hopper and press enter to continue") toc = time.time() delay += toc - tic n = 0 return delay
def get_bricks(c,ser_ee,brick,STATE): if brick['b']==0: # if 2x4 brick use hopper 0 if brick['p']==3: # if tool placing method, pick 2 bricks if STATE == EMPTY: feed_pick(c,ser_ee) feed_pick(c,ser_ee,stack=2) elif (brick['r'] == 0 or brick['r'] == 180) and STATE == EMPTY: feed_pick(c,ser_ee,X=brick['p']) elif STATE == EMPTY: feed_pick(c,ser_ee,X=2-brick['p']) elif brick['b']==1: # if 2x2 brick use hopper 1 if STATE == EMPTY: feed_pick(c,ser_ee,X=2,H=1) if brick['p'] == 3: feed_pick(c,ser_ee,X=2,H=1,stack=2) # home waypoint msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) return
def feed_pick(c, ser_ee, X=1, H=0, stack=1): ic.socket_send(c, sCMD=300) # select lego tcp (tool centre point) if H == 0: if X == 0: demand_Pose = dict(wp.Hopper0_feed0) elif X == 1: demand_Pose = dict(wp.Hopper0_feed1) else: demand_Pose = dict(wp.Hopper0_feed2) else: if X == 0: demand_Pose = dict(wp.Hopper1_feed0) elif X == 1: demand_Pose = dict(wp.Hopper1_feed1) else: demand_Pose = dict(wp.Hopper1_feed2) demand_Pose['z'] = demand_Pose['z'] + 20 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4) # move above brick demand_Pose['z'] = demand_Pose['z'] - 20 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=8) #demand_Pose['z'] = demand_Pose['z']-5 #msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4,Speed=0.5) ic.super_serial_send(ser_ee, "G", 49) demand_Pose['z'] = demand_Pose['z'] + 40 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4) while stack > 1: time.sleep(0.3) stack -= 1 demand_Pose['z'] = demand_Pose['z'] - 40 + 8 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4) ic.socket_send(c, sCMD=300 + stack) demand_Pose = dict(smooth_rotate(c, R=2)) demand_Pose['y'] = demand_Pose['y'] + 0.5 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=8) # rotate grabber demand_Pose['y'] = demand_Pose['y'] - 0.5 demand_Pose['z'] = demand_Pose['z'] + 40 msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4) return
def stow_bricks(c,ser_ee,input_bricks): bricks = list(input_bricks) STATE = 0 # stow bricks if bricks[0]['p'] == 3: feed_place2(c,ser_ee,sH=bricks[0]['b']) if len(bricks) != 1: # if tool picking next, don't stow brick if (bricks[0]['p'] == 3 or bricks[0]['p'] == 1) and bricks[1]['p'] == 3 and bricks[0]['b'] == 0: STATE = X4 elif bricks[1]['p'] == 3 and bricks[0]['b'] == 1: STATE = X2 else: feed_place(c,ser_ee,H=bricks[0]['b']) else: feed_place(c,ser_ee,H=bricks[0]['b']) # home waypoint msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2,Speed=1.05) return STATE
def main(): ipt = raw_input("Robot connected?(y/n)") if ipt == "y": c, ser_ee = initialize() # loop print c.recv(1024) inp = raw_input("Continue?") msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) while True: task = raw_input("task: ") if task == "o": ic.super_serial_send(ser_ee, "G", 51) if task == "c": ic.super_serial_send(ser_ee, "G", 49) if task == "cal": ipt = raw_input("Continue?") lm.grid_pick(c, ser_ee, 0, 1, 0, 0) lm.grid_place(c, ser_ee, 0, 1, 0, 0) ipt = raw_input("Continue?") lm.grid_pick(c, ser_ee, 30, 1, 0, 0) lm.grid_place(c, ser_ee, 30, 1, 0, 0) ipt = raw_input("Continue?") lm.grid_pick(c, ser_ee, 30, 13, 0, 0) lm.grid_place(c, ser_ee, 30, 13, 0, 0) ipt = raw_input("Continue?") lm.grid_pick(c, ser_ee, 0, 13, 0, 0) lm.grid_place(c, ser_ee, 0, 13, 0, 0) ipt = raw_input("Continue?") msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) if task == "stack": msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) time.sleep(3) lm.stack_pick(c, ser_ee, 0, 1, 2, 0, stack=3) lm.grid_place(c, ser_ee, 30, 1, 2, 0) if task == "rot": msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) ic.socket_send(c, sCMD=300) current_Pose = ic.get_ur_position(c, 1) demand_Pose = { "x": current_Pose[0], "y": current_Pose[1], "z": current_Pose[2], "rx": current_Pose[3], "ry": current_Pose[4], "rz": current_Pose[5] } print "current_Pose: ", demand_Pose demand_Pose = dict(lm.smooth_rotate(c, 0, R=20)) print "demand_Pose: ", demand_Pose ipt = raw_input("continue?") msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=8) # if task == "lego": # #time.sleep(5) # ipt = raw_input("Open?(y/n)") # if ipt == "y": # ic.super_serial_send(ser_ee,"G",51) # for i in range(0,3): # for j in range(0,4): # for k in range(0,2): # lm.grid_pick(c,ser_ee,j*8+3,k*6+1,4,0) # lm.grid_place(c,ser_ee,j*8+3,k*6+7,4,0) # msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2) # for j in range(0,4): # for k in range(0,2): # lm.grid_pick(c,ser_ee,27-j*8,13-k*6,4,0) # lm.grid_place(c,ser_ee,27-j*8,7-k*6,4,0) # msg = ic.safe_ur_move(c,Pose=dict(wp.home_joints),CMD=2) if task == "lego": ic.socket_send(c, sCMD=300) lm.grid_pick(c, ser_ee, 0, 1, 0, 0) if task == "pt": #time.sleep(5) ipt = raw_input("Open?(y/n)") if ipt == "y": ic.super_serial_send(ser_ee, "G", 51) for i in range(0, 4): for j in range(0, 4): for k in range(0, 2): lm.grid_pick(c, ser_ee, ((j + k) * 8 + 3) % 32, k * 6 + 1, 1, 0) lm.grid_place(c, ser_ee, ((j + k + 1) * 8 + 3) % 32, k * 6 + 7, 1, 0) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) for j in range(0, 4): lm.grid_pick(c, ser_ee, j * 8 + 3, 13, 1, 0) lm.grid_place(c, ser_ee, ((j + 1) * 8 + 3) % 32, 1, 1, 0) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) if task == "wp": msg = ic.safe_ur_move(c, Pose=dict(wp.feed_stow_wp_joints), CMD=2) if task == "pose": current_Pose = ic.get_ur_position(c, 1) print "current pose: ", current_Pose if task == "joints": current_Joints, current_Grip = ic.get_ur_position(c, 3) print "current joints: ", current_Joints if task == "dis": model = fd.import_file("example.txt") bricks = fd.decode_file(model) que, opt = dis.sort_bricks_dis(bricks, model) print opt print "x\ty\tp\tr\tex\tey" for i in range(0, len(que)): print que[i]['x'], '\t', que[i]['y'], '\t', que[i][ 'p'], '\t', que[i]['r'], '\t', que[i]['ye'], '\t', que[i][ 'xe'] lm.clean_disassemble(c, ser_ee, que) if task == "disre": ipt = raw_input("Open?(y/n)") if ipt == "y": ic.super_serial_send(ser_ee, "G", 51) model = fd.import_file("examplere.txt") bricks = fd.decode_file(model) que, opt = dis.sort_bricks_dis(bricks, model) print opt print "x\ty\tp\tr\tex\tey" for i in range(0, len(que)): print que[i]['x'], '\t', que[i]['y'], '\t', que[i][ 'p'], '\t', que[i]['r'], '\t', que[i]['ye'], '\t', que[i][ 'xe'] ipt = raw_input("continue?") #lm.assemble(c,ser_ee,bricks) #ipt = raw_input("continue?") lm.clean_disassemble(c, ser_ee, que) if task == "ass": model = fd.import_file("example.txt") bricks = fd.decode_file(model) que, opt = ass.sort_bricks_ass(bricks, model) print opt print "x\ty\tp\tr\tex\tey" for i in range(0, len(que)): print que[i]['x'], '\t', que[i]['y'], '\t', que[i][ 'p'], '\t', que[i]['r'], '\t', que[i]['ye'], '\t', que[i][ 'xe'] ipt = raw_input('Continue?') if ipt == 'y': lm.clean_assemble(c, ser_ee, que) if task == "assre": ipt = raw_input("Open?(y/n)") if ipt == "y": ic.super_serial_send(ser_ee, "G", 51) model = fd.import_file("examplere.txt") bricks = fd.decode_file(model) que, opt = ass.sort_bricks_ass(bricks, model) print opt for i in range(0, len(que)): if que[i]['r'] == 0 or que[i]['r'] == 180: print que[i]['x'], ', ', que[i]['y'] + que[i][ 'p'], ', ', que[i]['z'] else: print que[i]['x'] + que[i]['p'], ', ', que[i][ 'y'], ', ', que[i]['z'] ipt = raw_input("continue?") #lm.assemble(c,ser_ee,bricks) #ipt = raw_input("continue?") lm.assemble(c, ser_ee, que) if task == "i": model = fd.import_file("example.txt") #print get_size(model) if task == "stow": h = int(raw_input("H: ")) s = int(raw_input("stack: ")) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) if s == 2: lm.feed_place2(c, ser_ee, sH=h) lm.feed_place(c, ser_ee, H=h) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) if task == "pick": msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) h = int(raw_input("H: ")) x = int(raw_input("x: ")) s = int(raw_input("stack: ")) lm.feed_pick(c, ser_ee, X=x, H=h, stack=s) msg = ic.safe_ur_move(c, Pose=dict(wp.home_joints), CMD=2) if task == "ls": model = fd.import_file("example.txt") bricks = fd.decode_file(model) #tic = time.time() que, opt, p = ass.sort_bricks_ass(bricks, copy.deepcopy(model)) #print "sort time = ", time.time()-tic #print "Sort output: ", opt #print "total cost = ",ass.list_cost(que,copy.deepcopy(model)) #print "" #for i in range(0,len(que)): # if que[i]['r'] == 0 or que[i]['r'] == 180: # print que[i]['x'],', ',que[i]['y'],', ',que[i]['p'],', ',que[i]['r'],', ',que[i]['z'],', ',que[i]['ye'],', ',que[i]['xe'] # else: # print que[i]['x'],', ',que[i]['y'],', ',que[i]['p'],', ',que[i]['r'],', ',que[i]['z'],', ',que[i]['xe'],', ',que[i]['ye'] #for i in range(0,len(que)): # if que[i]['r'] == 0 or que[i]['r'] == 180: # print que[i]['x'],', ',que[i]['y']+que[i]['p'],', ',que[i]['z'],', ',que[i]['ye'],', ',que[i]['xe'] # else: # print que[i]['x']+que[i]['p'],', ',que[i]['y'],', ',que[i]['z'],', ',que[i]['xe'],', ',que[i]['ye'] #ipt = raw_input("continue?") #tic = time.time() #delay = lm.assemble(c,ser_ee,que) #tock = time.time() print "----------- assembly prob ----------" print " ", p * 100, '%' print "----------- assembly time ----------" print " ", 14.8 * len(que) #ipt = raw_input("continue?") #model = fd.import_file("example.txt") #bricks = fd.decode_file(model) #tic = time.time() que, opt = dis.sort_bricks_dis(bricks, copy.deepcopy(model)) #print "dis time = ", time.time()-tic print "Sort output: ", opt #print "total cost = ",ass.list_cost(que,copy.deepcopy(model)) #for i in range(0,len(que)): # if que[i]['r'] == 0 or que[i]['r'] == 180: # print que[i]['x'],', ',que[i]['y'],', ',que[i]['p'],', ',que[i]['r'],', ',que[i]['z'],', ',que[i]['ye'],', ',que[i]['xe'] # else: # print que[i]['x'],', ',que[i]['y'],', ',que[i]['p'],', ',que[i]['r'],', ',que[i]['z'],', ',que[i]['xe'],', ',que[i]['ye'] #tic1 = time.time() #lm.disassemble(c,ser_ee,que) #toc1 = time.time() if opt == 'y': print "--------- disassembly prob --------" print " ", (0.999**len(que)) * 100, '%' if opt == 'n': print "--------- disassembly prob ---------" print " ", 0, '%' print "--------- disassembly time --------" print " ", 20.8 * len(que) print "" print "" print "" print "" if task == "lsflex": model = fd.import_file("example.txt") bricks = fd.decode_file(model) flex, updated_model, opt = flx.sort_bricks_flex(bricks, model) que, opt, p = ass.sort_bricks_ass(flex, copy.deepcopy(updated_model)) print "----------- assembly prob ----------" print " ", p * 100, '%' print "----------- assembly time ----------" print " ", 14.8 * len(que) que, opt = dis.sort_bricks_dis(flex, copy.deepcopy(updated_model)) if opt == 'y': print "--------- disassembly prob --------" print " ", (0.999**len(que)) * 100, '%' if opt == 'n': print "--------- disassembly prob ---------" print " ", 0, '%' print "--------- disassembly time --------" print " ", 20.8 * len(que) print "" print "" print "" print "" if task == "re": model1 = fd.import_file("example.txt") bricks1 = fd.decode_file(model1) model2 = fd.import_file("examplere.txt") bricks2 = fd.decode_file(model2) dis_list, ass_list = rea.reassemble(bricks1, bricks2, model1, model2) #print "dis_list :",dis_list #print "as_list :",ass_list #ipt = raw_input("continue?") que, opt = dis.sort_bricks_dis(dis_list, copy.deepcopy(model1)) print "disassembly sort output: ", opt print "-------------- remove --------------" print "x", ",", " y", ",", " p", ",", " r", ",", " z" for i in range(0, len(que)): if que[i]['r'] == 0 or que[i]['r'] == 180: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'] else: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'] ipt = raw_input("continue?") lm.disassemble(c, ser_ee, que) print "" print "" que, opt = ass.sort_bricks_ass(ass_list, copy.deepcopy(model2)) print "assembly sort output: ", opt print "-------------- build --------------" print "x", ",", " y", ",", " p", ",", " r", ",", " z", ",", " ex", ",", " ey" for i in range(0, len(que)): if que[i]['r'] == 0 or que[i]['r'] == 180: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i][ 'z'], ', ', -que[i]['ye'], ', ', que[i]['xe'] else: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'], ', ', que[ i]['xe'], ', ', que[i]['ye'] ipt = raw_input("continue?") lm.assemble(c, ser_ee, que) if task == 'flex': ipt = int(raw_input("model1?")) if ipt == 1: model = fd.import_file("model1.txt") elif ipt == 2: model = fd.import_file("model2.txt") elif ipt == 3: model = fd.import_file("model3.txt") elif ipt == 4: model = fd.import_file("model4.txt") elif ipt == 5: model = fd.import_file("model5.txt") elif ipt == 6: model = fd.import_file("model6.txt") bricks = fd.decode_file(model) que, updated_model, opt = flx.sort_bricks_flex(bricks, model) print "flex sort output: ", opt print "-------------- build --------------" print "x", ",", " y", ",", " p", ",", " r", ",", " z", ",", "ex", ",", "ey", ",", " b" for i in range(0, len(que)): if que[i]['r'] == 0 or que[i]['r'] == 180: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'], ', ', -que[ i]['ye'], ', ', que[i]['xe'], ', ', que[i]['b'] else: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'], ', ', que[ i]['xe'], ', ', que[i]['ye'], ', ', que[i]['b'] ipt = raw_input("continue?") que, opt = ass.sort_bricks_ass(que, copy.deepcopy(updated_model)) print "assembly sort output: ", opt print "-------------- build --------------" print "x", ",", " y", ",", " p", ",", " r", ",", " z", ",", "ex", ",", "ey", ",", " b" for i in range(0, len(que)): if que[i]['r'] == 0 or que[i]['r'] == 180: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'], ', ', -que[ i]['ye'], ', ', que[i]['xe'], ', ', que[i]['b'] else: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'], ', ', que[ i]['xe'], ', ', que[i]['ye'], ', ', que[i]['b'] print ass.list_cost(que, updated_model) ipt = raw_input("continue?") tic = time.time() delay = lm.assemble(c, ser_ee, que) tock = time.time() print "----------- assembly time ----------" print " ", tock - tic - delay print "------------- resources ------------" print " ", len(que) #que,opt = dis.sort_bricks_dis(que,copy.deepcopy(updated_model)) #print "disassembly sort output: ",opt #print "-------------- build --------------" #print "x",","," y",","," p",","," r",","," z",",","ex",",","ey",","," b" #for i in range(0,len(que)): # if que[i]['r'] == 0 or que[i]['r'] == 180: # print que[i]['x'],', ',que[i]['y'],', ',que[i]['p'],', ',que[i]['r'],', ',que[i]['z'],', ',-que[i]['ye'],', ',que[i]['xe'],', ',que[i]['b'] # else: # print que[i]['x'],', ',que[i]['y'],', ',que[i]['p'],', ',que[i]['r'],', ',que[i]['z'],', ',que[i]['xe'],', ',que[i]['ye'],', ',que[i]['b'] #ipt = raw_input("continue?") #lm.disassemble(c,ser_ee,que) if task == "reflex": ipt = int(raw_input("model1?")) if ipt == 1: model1 = fd.import_file("model1.txt") model2 = fd.import_file("model2.txt") elif ipt == 2: model1 = fd.import_file("model2.txt") model2 = fd.import_file("model3.txt") elif ipt == 3: model1 = fd.import_file("model3.txt") model2 = fd.import_file("model4.txt") elif ipt == 4: model1 = fd.import_file("model4.txt") model2 = fd.import_file("model5.txt") elif ipt == 5: model1 = fd.import_file("model5.txt") model2 = fd.import_file("model6.txt") bricks1 = fd.decode_file(model1) #print "1 length: ", len(bricks1) bricks2 = fd.decode_file(model2) #print "2 length: ", len(bricks2) flex1, updated_model1, opt1 = flx.sort_bricks_flex(bricks1, model1) flex2, updated_model2, opt2 = flx.sort_bricks_flex(bricks2, model2) dis_list, ass_list = rea.reassemble(flex1, flex2, updated_model1, updated_model2) print "dis_list :", dis_list print "as_list :", ass_list ipt = raw_input("continue?") que, opt = dis.sort_bricks_dis(dis_list, copy.deepcopy(updated_model1)) print "disassembly sort output: ", opt print "-------------- remove --------------" print "x", ",", " y", ",", " p", ",", " r", ",", " z" for i in range(0, len(que)): if que[i]['r'] == 0 or que[i]['r'] == 180: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'] else: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'] ipt = raw_input("continue?") tic1 = time.time() lm.disassemble(c, ser_ee, que) toc1 = time.time() bricks_saved = len(que) #print "dis length: ", bricks_saved #print "" #print "" que, opt = ass.sort_bricks_ass(ass_list, copy.deepcopy(updated_model2)) #print "ass length: ", len(que) #print "P(success): ",p #print "" print "assembly sort output: ", opt print "-------------- build --------------" print "x", ",", " y", ",", " p", ",", " r", ",", " z", ",", " ex", ",", " ey" for i in range(0, len(que)): if que[i]['r'] == 0 or que[i]['r'] == 180: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i][ 'z'], ', ', -que[i]['ye'], ', ', que[i]['xe'] else: print que[i]['x'], ', ', que[i]['y'], ', ', que[i][ 'p'], ', ', que[i]['r'], ', ', que[i]['z'], ', ', que[ i]['xe'], ', ', que[i]['ye'] ipt = raw_input("continue?") tic2 = time.time() delay = lm.assemble(c, ser_ee, que) toc2 = time.time() print "---------- reassembly time ---------" print " ", toc2 - tic2 - delay + toc1 - tic1 print "------------- resources ------------" print " ", len(que) - bricks_saved if task == 'time': tic = time.time() ipt = raw_input("Refill hopper and press enter to continue") toc = time.time() print toc - tic if task == 'gol': nbricks = int(raw_input("Number of starting bricks: ")) iter = int(raw_input("Number of iterations: ")) gol.game_of_life(c, ser_ee, nbricks, iter)