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)
