def grab_pick(c,ser_ee,ser_vac,ser_led,y,z=12,orientation=0,object_height=30.0,size=70,xoff=0,zoff=0,n=0,obj=6,stored_x=0):
# curved object picking strategy
#ic.socket_send(c,sCMD=101)
stored_z=z
shelf = 0
if z < 247:
z = 151.3
if y < -215:
shelf = 4
else:
shelf = 3
else:
z = 359.7
if y < -445:
shelf = 2
elif y < -215:
shelf = 1
else:
shelf = 0
y = y + orientation/2
if shelf == 0:
if y > -30.0: y = -30.0
if y < -172.0: y = -172.0
if shelf == 1:
if y > -265.0: y = -265.0
if y < -395.0: y = -395.0
if shelf == 2:
if y > -490.0: y = -490.0
if y < -629.0: y = -629.0
if shelf == 3:
if y > -30.0: y = -30.0
if y < -172.0: y = -172.0
if shelf == 4:
if y > -265.0: y = -265.0
if y < -629.0: y = -629.0
#print "y: ", y
#print "z: ", z
# Send end effector cmd without waiting for a reply
demand_Grip = dict(uw.end_effector_home)
demand_Grip["act"] = int(80.0-0.6*object_height)
ser_ee.flush
print "Sending actuator move"
ser_ee.write("A" + chr(demand_Grip["act"]) + "\n")
if obj==6 or obj==8 or obj==9:
# Move to shelf
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_joints[shelf]),CMD=2)
# Align with object
current_Pose = ic.get_ur_position(c,1)
zoffset=3*(shelf%3)-4+zoff
demand_Pose = {"x":current_Pose[0],"y":y,"z":current_Pose[2]+object_height+zoffset+5,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Wait for end effector to finish adjustment
while True:
ipt = ser_ee.readline()
print ipt
if ipt == "done\r\n":
break
timeout = ser_ee.readline()
print "timeout: ", timeout
ser_ee.flush
xoffset=10*(2-shelf%3)+xoff
demand_Pose["x"]=current_Pose[0]+185.0+xoffset
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.25,CMD=8)
demand_Pose["z"]=current_Pose[2]+object_height+zoffset-20
demand_Grip["servo"]=80
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.2,CMD=4)
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.2,CMD=4)
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.2,CMD=4)
demand_Pose["x"]=current_Pose[0]+110+xoffset
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.05,CMD=8)
demand_Pose["x"]=current_Pose[0]+113+xoffset
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.05,CMD=8)
demand_Grip["servo"]=0
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.25,CMD=8)
clr = copy.deepcopy(uw.empty_led)
clr[3]=[0,0,255]
clr[4]=[0,0,255]
lf.illuminate_cluster(ser_led,2,colour=clr)
demand_Pose["z"]=current_Pose[2]+object_height+zoffset+30
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.10,CMD=8)
demand_Pose["x"]=current_Pose[0]-10+xoffset
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.15,CMD=8)
elif obj==7 or obj==10:
# Reset tool to tcp_1
ic.socket_send(c,sCMD=101)
# Move to shelf
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_grab_joints[shelf]),CMD=2)
# Align with object
current_Pose = ic.get_ur_position(c,1)
xoffset=10*(2-shelf%3)+xoff
demand_Pose = {"x":current_Pose[0]+xoffset,"y":y,"z":stored_z,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Wait for end effector to finish adjustment
while True:
ipt = ser_ee.readline()
print ipt
if ipt == "done\r\n":
break
timeout = ser_ee.readline()
print "timeout: ", timeout
ser_ee.flush
demand_Grip["act"] = int(80.0-0.6*object_height)-12
demand_Pose["x"]=stored_x+10
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.1,CMD=8)
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.2,CMD=4)
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.2,CMD=4)
for i in range(0,81):
#print "Sending end effector move"
ser_ee.flush
# Set Actuator position, min = 0, max = 80
ser_ee.write("G" + chr(80-i) + "\n")
# Wait for end effector arduino to finish
while True:
ipt = ser_ee.readline()
#print ipt
if ipt == "done\r\n":
break
demand_Grip["act"] = int(80.0-0.6*object_height)
demand_Grip["servo"] = 0
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.2,CMD=4)
clr = copy.deepcopy(uw.empty_led)
clr[3]=[0,0,255]
clr[4]=[0,0,255]
lf.illuminate_cluster(ser_led,2,colour=clr)
demand_Pose["x"]=current_Pose[0]-30+xoffset
demand_Pose["z"]=stored_z+20
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,Speed=0.25,CMD=8)
# Move away from shelf
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_joints_waypoint),CMD=2)
# Rotate base
current_Joints = ic.get_ur_position(c,3)
demand_Joints = {"x":current_Joints[0]-90,"y":current_Joints[1],"z":current_Joints[2],"rx":current_Joints[3],"ry":current_Joints[4],"rz":current_Joints[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Joints),CMD=2)
# Move to new position
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":-150*(4-n),"y":current_Pose[1],"z":current_Pose[2]-100+object_height,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Release object
demand_Grip["servo"]=50
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
time.sleep(0.3)
demand_Grip["servo"]=30
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
demand_Grip["servo"]=50
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
# Release object
#demand_Grip["servo"]=30
#msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
time.sleep(0.2)
# Release object
demand_Grip["servo"]=80
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
clr = copy.deepcopy(uw.empty_led)
lf.illuminate_cluster(ser_led,2,colour=clr)
time.sleep(0.2)
demand_Pose["z"]=current_Pose[2]+50
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Reset tool to tcp_1
ic.socket_send(c,sCMD=100)
# Return home
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(uw.grab_home_joints),Grip=demand_Grip,CMD=2)
return "Completed pick from shelf "+ str(shelf)
def grab_stow(c,ser_ee,ser_vac,ser_led,x,y,z=25,orientation=0,angle_of_attack=0,shelf=0,size=40,xoff=0,zoff=0,obj=6):
# Select tcp_2, for rotations around the grasping point
ic.socket_send(c,sCMD=103)
object_size = 80-int(size)
if object_size < 5: object_size=5
demand_Grip = dict(uw.end_effector_home)
demand_Grip["act"]=object_size-5
ser_ee.flush
print "Sending actuator move"
ser_ee.write("A" + chr(demand_Grip["act"]) + "\n")
# Break-up rotations into max 90degrees
thetaz = 0
if orientation>90:
orientation=orientation-90
thetaz=math.pi/2
elif orientation<-90:
orientation=orientation+90
thetaz=-math.pi/2
# Avoid singularity at +/-45degrees
if orientation==45:
orientation = 44
elif orientation==-45:
orientation = -44
# Convert to radians
angle_of_attack=angle_of_attack*math.pi/180.0
orientation=orientation*math.pi/180.0
thetay=135.0*math.pi/180.0
# Cartesian rotation matrices to match uw.grabbing_joints rotation
x_rot = np.matrix([[ 1.0, 0.0, 0.0],
[ 0.0, math.cos(math.pi/2), -math.sin(math.pi/2)],
[ 0.0, math.sin(math.pi/2), math.cos(math.pi/2)]]) # x_rot[rows][columns]
y_rot = np.matrix([[ math.cos(thetay), 0.0, -math.sin(thetay)],
[ 0.0, 1.0, 0.0],
[ math.sin(thetay), 0.0, math.cos(thetay)]]) # y_rot[rows][columns]
z_rot = np.matrix([[ math.cos(0.0), -math.sin(0.0), 0.0],
[ math.sin(0.0), math.cos(0.0), 0.0],
[ 0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Move to grabbing waypoint
msg = ic.safe_ur_move(c,Pose=dict(uw.grabbing_joints_waypoint),Speed=1.0,CMD=2)
# Create rotation matrix for current position
R=z_rot*y_rot*x_rot
if thetaz!=0:
# Axis rotation matricies for grasping position, rotate around x-axis by aoa, then z-axis by ori
x_rot = np.matrix([[ 1.0, 0.0, 0.0],
[ 0.0, math.cos(angle_of_attack), -math.sin(angle_of_attack)],
[ 0.0, math.sin(angle_of_attack), math.cos(angle_of_attack)]]) # x_rot[rows][columns]
z_rot = np.matrix([[ math.cos(thetaz), -math.sin(thetaz), 0.0],
[ math.sin(thetaz), math.cos(thetaz), 0.0],
[ 0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Cartesian rotation matrix of desired orientation
R=z_rot*x_rot*R
# Cartesian to axis-angle
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1.0)/2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta * 180/math.pi
ry = multi * (R[0, 2] - R[2, 0]) * theta * 180/math.pi
rz = multi * (R[1, 0] - R[0, 1]) * theta * 180/math.pi
print rx, ry, rz
# Rotate around tool centre point defined by tcp_2
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":current_Pose[2],"rx":rx,"ry":ry,"rz":rz}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=8)
# Axis rotation matricies for grasping position, rotate around x-axis by aoa, then z-axis by ori
z_rot = np.matrix([[ math.cos(orientation), -math.sin(orientation), 0.0],
[ math.sin(orientation), math.cos(orientation), 0.0],
[ 0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Cartesian rotation matrix of desired orientation
R=z_rot*R
# Cartesian to axis-angle
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1.0)/2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta * 180/math.pi
ry = multi * (R[0, 2] - R[2, 0]) * theta * 180/math.pi
rz = multi * (R[1, 0] - R[0, 1]) * theta * 180/math.pi
print rx, ry, rz
# Rotate around tool centre point defined by tcp_2
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":current_Pose[2],"rx":rx,"ry":ry,"rz":rz}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=8)
else:
# Axis rotation matricies for grasping position, rotate around x-axis by aoa, then z-axis by ori
x_rot = np.matrix([[ 1.0, 0.0, 0.0],
[ 0.0, math.cos(angle_of_attack), -math.sin(angle_of_attack)],
[ 0.0, math.sin(angle_of_attack), math.cos(angle_of_attack)]]) # x_rot[rows][columns]
z_rot = np.matrix([[ math.cos(orientation), -math.sin(orientation), 0.0],
[ math.sin(orientation), math.cos(orientation), 0.0],
[ 0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Cartesian rotation matrix of desired orientation
R=z_rot*x_rot*R
# Cartesian to axis-angle
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1.0)/2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta * 180/math.pi
ry = multi * (R[0, 2] - R[2, 0]) * theta * 180/math.pi
rz = multi * (R[1, 0] - R[0, 1]) * theta * 180/math.pi
print rx, ry, rz
# Rotate around tool centre point defined by tcp_2
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":current_Pose[2],"rx":rx,"ry":ry,"rz":rz}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=8)
while True:
ipt = ser_ee.readline()
print ipt
if ipt == "done\r\n":
break
timeout = ser_ee.readline()
print "timeout: ", timeout
ser_ee.flush
# Move to above object
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":x,"y":y,"z":z+100,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4,Speed=0.5)
# Move closer
#demand_Pose = {"x":x,"y":y,"z":z+50,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
#msg = ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4,Speed=0.5)
# Linear move to grasping position
demand_Pose = {"x":x,"y":y,"z":z,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
demand_Grip["servo"]=30
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=8,Speed=0.4)
# Grab with reduced chance of collision
# Partially close grabber servo
current_Pose, current_Grip = ic.get_position(c,ser_ee,ser_vac,CMD=1)
demand_Grip = {"act": object_size, "servo": 30, "tilt": current_Grip[2]&0x20, "vac": current_Grip[4]}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
# Adjust actuator position
demand_Grip["act"]=object_size+4
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
# Open grabber servo
#demand_Grip["servo"]=80
#msg = ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
clr = copy.deepcopy(uw.empty_led)
clr[3]=[0,0,255]
clr[4]=[0,0,255]
lf.illuminate_cluster(ser_led,2,colour=clr)
# Close grabber servo
demand_Grip["servo"]=0
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
time.sleep(0.5)
# Lift object
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":z+100,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
ic.socket_send(c,sCMD=101)
# Move safely towards shelf
msg = ic.safe_ur_move(c,Pose=dict(uw.grabbing_joints_waypoint),CMD=2)
# Move safely towards shelf
current_Joints = ic.get_ur_position(c,3)
demand_Joints = {"x":90,"y":current_Joints[1],"z":current_Joints[2],"rx":current_Joints[3],"ry":current_Joints[4],"rz":current_Joints[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Joints), CMD=2)
if obj==6 or obj==8 or obj==9:
# Move safely towards shelf
demand_Joints = dict(uw.shelf_joints_waypoint)
demand_Joints["rz"] = demand_Joints["rz"]-90
msg = ic.safe_ur_move(c,Pose=dict(demand_Joints),CMD=2)
# Move safely towards shelf
demand_Joints = dict(uw.shelf_joints[shelf])
demand_Joints["rz"] = demand_Joints["rz"]-90
msg = ic.safe_ur_move(c,Pose=dict(demand_Joints),CMD=2)
# Define position on shelf
object_height = 50
zoffset = 3*(shelf%3)-4+zoff
shelf_depth = 766-10*(shelf%3)
yoff = 35
# Align object with shelf
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1]+yoff,"z":current_Pose[2]+object_height+zoffset,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Move into shelf
demand_Pose["x"]=shelf_depth+xoff
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Move into shelf
demand_Pose["z"]=current_Pose[2]+object_height-15+zoffset
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
elif obj==7 or obj ==10:
# Move safely towards shelf
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_grab_joints[shelf]),CMD=2)
# Define position on shelf
object_height = 50
zoffset = 3*(shelf%3)-4+zoff
shelf_depth = 766-10*(shelf%3)
yoff = 0
# Align object with shelf
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1]+yoff,"z":current_Pose[2]+object_height+zoffset,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Move into shelf
demand_Pose["x"]=shelf_depth+xoff
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Move into shelf
demand_Pose["z"]=current_Pose[2]+object_height-15+zoffset
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Release object
#demand_Grip["servo"]=10
#msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
if obj==7:
#demand_Grip["servo"]=23
#msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
demand_Grip["act"]=object_size-2
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
for i in range(0,81):
#print "Sending end effector move"
ser_ee.flush
# Set Actuator position, min = 0, max = 80
ser_ee.write("G" + chr(i) + "\n")
# Wait for end effector arduino to finish
while True:
ipt = ser_ee.readline()
#print ipt
if ipt == "done\r\n":
break
#time.sleep(0.005*(80-i))
#demand_Grip["servo"]=20
#msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
#demand_Grip["servo"]=80
#msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
pick_Pose = ic.get_ur_position(c,1)
x = pick_Pose[0]
y = pick_Pose[1]
z = pick_Pose[2]
clr = copy.deepcopy(uw.empty_led)
lf.illuminate_cluster(ser_led,2,colour=clr)
time.sleep(0.2)
# Move back
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1]+yoff,"z":current_Pose[2]+object_height+zoffset,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Return home
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_joints[shelf]),CMD=2)
# Return home
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_joints_waypoint),CMD=2)
# Reset tool to tcp_1
ic.socket_send(c,sCMD=100)
# Return home
msg = ic.safe_ur_move(c,Pose=dict(uw.grab_home_joints),CMD=2)
return "Shelf "+ str(shelf) + " completed",x,y,z
def vac_stow(c,ser_ee,ser_vac,ser_led,x,y,shelf,z=110,yoff=0):
# Set tool to tcp_5
ic.socket_send(c,sCMD=104)
# Home
demand_Pose = dict(uw.grab_home)
demand_Grip = dict(uw.end_effector_home)
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(uw.grab_home_joints),Grip=demand_Grip,CMD=2)
# Rotate end effector
current_Joints = ic.get_ur_position(c,3)
demand_Joints = {"x":current_Joints[0],"y":current_Joints[1],"z":current_Joints[2],"rx":current_Joints[3],"ry":current_Joints[4]+60,"rz":current_Joints[5]-45}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Joints),Grip=demand_Grip,CMD=2)
# Avoid camera mount
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":150,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
# Rotate base
current_Joints = ic.get_ur_position(c,3)
demand_Joints = {"x":current_Joints[0]-90,"y":current_Joints[1],"z":current_Joints[2],"rx":current_Joints[3],"ry":current_Joints[4],"rz":current_Joints[5]}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Joints),Grip=demand_Grip,CMD=2)
# Move to above the object
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":x,"y":y,"z":110,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
demand_Pose["z"]=z
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
clr = copy.deepcopy(uw.empty_led)
clr[4]=[255,0,0]
clr[5]=[255,0,0]
lf.illuminate_cluster(ser_led,2,colour=clr)
# Move down until oject is reached
demand_Pose = {"x":x,"y":y,"z":0,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
print "sending force_move................................................"
object_height = 1000.0*float(ic.safe_ur_move(c,Pose=dict(demand_Pose),Speed=0.05,CMD=5))-22.0
print "object_height: ", object_height
time.sleep(0.5)
# Turn on vacuum at current position
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]}
demand_Grip["vac"]="g"
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
time.sleep(2)
# Move to above object
demand_Pose = {"x":x,"y":y,"z":100+object_height,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),Speed=0.25,CMD=4)
# Rotate base to shelves
current_Joints = ic.get_ur_position(c,3)
demand_Joints = {"x":90,"y":current_Joints[1],"z":current_Joints[2],"rx":current_Joints[3],"ry":current_Joints[4],"rz":current_Joints[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Joints),CMD=2)
# Reset tool to tcp_1
ic.socket_send(c,sCMD=100)
# Move closer to shelves
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_joints_waypoint),CMD=2)
# Move to specific shelf
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_joints[shelf]),CMD=2)
# Set depth on shelf
if object_height < 57:
shelf_depth = 816
elif object_height < 100:
shelf_depth = 788
else:
shelf_depth = 650
print "Object too tall"
# Raise end to object_height above the shelf
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1]+yoff,"z":current_Pose[2]+object_height+10,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Move to back of shelf
demand_Pose = {"x":shelf_depth,"y":current_Pose[1]+yoff,"z":current_Pose[2]+object_height,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=8)
time.sleep(0.5)
# Release vacuum at current position
demand_Grip["vac"]="r"
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
clr = copy.deepcopy(uw.empty_led)
lf.illuminate_cluster(ser_led,2,colour=clr)
time.sleep(1.2)
pick_Pose = ic.get_ur_position(c,1)
x = pick_Pose[0]
y = pick_Pose[1]
z = pick_Pose[2]
# Exit shelf
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1]+yoff,"z":current_Pose[2]+object_height,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Return home
msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_joints[shelf]),CMD=2)
# Return home
#msg = ic.safe_ur_move(c,Pose=dict(uw.shelf_joints_waypoint),CMD=2)
# Return home
msg = ic.safe_ur_move(c,Pose=dict(uw.grab_home_joints),CMD=2)
print "object_height: ", object_height
return "Shelf "+ str(shelf) + " completed",x,y,z
def vac_pick(c,ser_ee,ser_vac,ser_led,y,z,n,x=816.3):
# Determine closest shelf
shelf = 0
if z < 247:
z = 151.3
z = 148.0
if y < -215:
shelf = 4
else:
shelf = 3
else:
z = 359.7
z = 356.0
if y < -445:
shelf = 2
elif y < -215:
shelf = 1
else:
shelf = 0
# Ensure co-ordinates are within shelf limits
if shelf == 0:
if y > -30.0: y = -30.0
if y < -172.0: y = -172.0
if shelf == 1:
if y > -265.0: y = -265.0
if y < -395.0: y = -395.0
if shelf == 2:
if y > -490.0: y = -490.0
if y < -629.0: y = -629.0
if shelf == 3:
if y > -30.0: y = -30.0
if y < -172.0: y = -172.0
if shelf == 4:
if y > -265.0: y = -265.0
if y < -629.0: y = -629.0
print "y: ", y
print "z: ", z
# Home
demand_Pose = dict(uw.grab_home)
demand_Grip = dict(uw.end_effector_home)
#msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(uw.shelf_home_joints),Grip=demand_Grip,CMD=2)
# Move to shelves
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(uw.shelf_joints_waypoint),Grip=demand_Grip,CMD=2)
# Move to shelf
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(uw.shelf_joints[shelf]),Grip=demand_Grip,CMD=2)
# Align with object
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":y,"z":z,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
object_pos = dict(demand_Pose)
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
# Move above object
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":x,"y":y,"z":z,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=8)
clr = copy.deepcopy(uw.empty_led)
clr[4]=[255,0,0]
clr[5]=[255,0,0]
lf.illuminate_cluster(ser_led,2,colour=clr)
# Move down until object is reached
demand_Pose = {"x":x,"y":y,"z":z-75,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
print "sending force_move................................................"
object_height = 1000.0*float(ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Speed=0.05,Grip=demand_Grip,CMD=5))
print "object_height: ", object_height
# Grab at current position
demand_Grip["vac"]="g"
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]}
msg = ic.safe_move(c, ser_ee, ser_vac, Pose=demand_Pose, Grip=demand_Grip, CMD=4)
time.sleep(2)
# Lift object
demand_Pose["z"]=z
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Speed=0.25,Grip=demand_Grip,CMD=4)
# Exit shelf
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(object_pos),Speed=0.75,Grip=demand_Grip,CMD=4)
# Move away from shelf
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(uw.shelf_joints_waypoint),Grip=demand_Grip,CMD=2)
# Rotate base
current_Joints = ic.get_ur_position(c,3)
demand_Joints = {"x":current_Joints[0]-90,"y":current_Joints[1],"z":current_Joints[2],"rx":current_Joints[3],"ry":current_Joints[4],"rz":current_Joints[5]}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Joints),Grip=demand_Grip,CMD=2)
# Move to new position
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":-150*(4-n),"y":current_Pose[1],"z":current_Pose[2],"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
# Release object
demand_Grip["vac"]="r"
msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
clr = copy.deepcopy(uw.empty_led)
lf.illuminate_cluster(ser_led,2,colour=clr)
time.sleep(1.2)
# Return home
#msg = ic.safe_move(c,ser_ee,ser_vac,Pose=dict(uw.shelf_home_joints),Grip=demand_Grip,CMD=2)
# Return home
msg = ic.safe_ur_move(c,Pose=dict(uw.grab_home_joints),CMD=2)
return "Completed pick from shelf "+ str(shelf)
def main():
c, ser_ee, ser_vac, ser_led = initialize()
# loop
ic.led_serial_send(ser_led, "I", 2, 0, 2, 0)
print c.recv(1024)
inp = raw_input("Continue?")
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
CMD=2)
##################### Vision Initialise #####################################
directory = PATH_TO_KINECT_IMAGES_DIR
cali = kv.load_npz_as_array("im_array_cal_FINAL", directory)
empt = kv.load_npz_as_array("im_array_empty_FINAL", directory)
empt_all = kv.prepare_im_array(empt)
cali_all = kv.prepare_im_array(cali)
depth_cali = run_calibration(empt_all, cali_all, adjust=False)
rgb_cali = run_calibration_rgb(empt_all,
cali_all,
depth_cali,
adjust=False)
while True:
task = raw_input("task: ")
if task == "lf":
lf.illuminate_cluster(ser_led,
7,
colour=[[255, 255, 255], [255, 255, 255],
[255, 255, 255], [255, 255, 255],
[255, 255, 255], [255, 255, 255]])
################ Calibration Step if needed #######################################
if task == "calibrate":
calibrate_check1 = raw_input("Prepared for Empty Capture?: ")
if calibrate_check1 == "yes":
print "empty"
empt = kv.capture_frames()
plt.imshow(empt['ir'])
plt.show()
empt_all = kv.prepare_im_array(empt)
rgb, depth, ir = empt_all
np.savez(os.path.join(PATH_TO_KINECT_IMAGES_DIR,
'im_array_empty_FINAL'),
rgb=rgb,
depth=depth,
ir=ir)
calibrate_check2 = raw_input("Prepared for Calibrate Capture?: ")
if calibrate_check2 == "yes":
cali = kv.capture_frames()
print "cali"
cali_all = kv.prepare_im_array(cali)
rgb, depth, ir = cali_all
np.savez(os.path.join(PATH_TO_KINECT_IMAGES_DIR,
'im_array_cal_FINAL'),
rgb=rgb,
depth=depth,
ir=ir)
directory = PATH_TO_KINECT_IMAGES_DIR
cali = kv.load_npz_as_array("im_array_cal_FINAL", directory)
empt = kv.load_npz_as_array("im_array_empty_FINAL", directory)
empt_all = kv.prepare_im_array(empt)
cali_all = kv.prepare_im_array(cali)
rgb, depth, ir = empt_all
rgb, depth, ir = cali_all
depth_cali = run_calibration(empt_all, cali_all, adjust=True)
rgb_cali = run_calibration_rgb(empt_all,
cali_all,
depth_cali,
adjust=True)
if task == "gh":
# Set tool to tcp_5
socket_send(c, sCMD=104)
# Home
demand_Pose = dict(uw.grab_home)
demand_Grip = dict(uw.end_effector_home)
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
# Rotate end effector
current_Joints = ic.get_ur_position(c, 3)
demand_Joints = {
"x": current_Joints[0],
"y": current_Joints[1],
"z": current_Joints[2],
"rx": current_Joints[3],
"ry": current_Joints[4] + 60,
"rz": current_Joints[5] - 45
}
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Joints),
Grip=demand_Grip,
CMD=2)
# Avoid camera mount
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": 150,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Grip=demand_Grip,
CMD=4)
# Rotate base
current_Joints = ic.get_ur_position(c, 3)
demand_Joints = {
"x": current_Joints[0] - 90,
"y": current_Joints[1],
"z": current_Joints[2],
"rx": current_Joints[3],
"ry": current_Joints[4],
"rz": current_Joints[5]
}
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Joints),
Grip=demand_Grip,
CMD=2)
x = float(raw_input("x: "))
y = float(raw_input("y: "))
# Move to above the object
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": x,
"y": y,
"z": 100,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Grip=demand_Grip,
CMD=4)
if task == "ls":
shelf = int(raw_input("cluster: "))
ic.led_serial_send(ser_led, "C", shelf, 255, 255, 255)
for i in range(0, 6):
if i != shelf:
ic.led_serial_send(ser_led, "C", i, 0, 0, 0)
ic.led_serial_send(ser_led, "S", 1, 0, 0, 0)
if task == "led":
while True:
#ic.led_serial_send(ser_led,"I",3,127,0,0)
cmd = raw_input("cmd: ")
shelf = int(raw_input("cluster: "))
r = int(raw_input("r: "))
g = int(raw_input("g: "))
b = int(raw_input("b: "))
ic.led_serial_send(ser_led, cmd, shelf, r, g, b)
if task == "gp":
while (1):
capture_check = raw_input("Ready?: ")
if capture_check == "yes":
test = kv.capture_frames()
test_all = kv.prepare_im_array(test)
break
######## Process Test Image and Retrieve Depth and Contour Information from Depth and RGB Data ##########
rgb, depth, ir = test_all
normclean, sorted_family = run_image_processing_v2_depth(
test_all, depth_cali, show=False)
rgbnormclean, rgb_family, test_rgbx_img = run_image_processing_v2_rgb(
test_all, rgb_cali, depth_cali, show=False)
######## Clean the images and convert them so that they are cv2 compatible ############
depth_normclean = normclean2cv2(normclean)
rgb_normclean = normclean2cv2(rgbnormclean)
test_rgb_img = vt.convert2rgb(test_rgbx_img)
cv2.imwrite("test_rgb_img.jpg", test_rgb_img)
####### Create List of Objects and match the rgb and depth data ##########
object_list = match_rgb_with_depth_v2(sorted_family, rgb_family,
depth_normclean,
test_rgb_img)
#cv2.imwrite("test_rgb_img.jpg", test_rgb_img)
cv2.imwrite("depth_normclean.jpg", depth_normclean)
import object_recognition_tools as ort
excluded_val = ['centre', 'rgb_centre', 'number of children']
extras = ['R', 'G', 'B', 'centre_offset']
obj_features_mean, obj_features_std = ort.prepare_obj_features_param(
obj_feat_csv='object_features.csv',
excluded_val=excluded_val,
extras=extras)
rec_df = ort.prepare_pick_obj_features_param(
object_list, excluded_val, extras)
cost_list = ort.create_cost_list(obj_features_mean,
obj_features_std, rec_df)
object_list = ort.label_object_list(object_list,
cost_list,
test_rgb_img,
show=SHOW)
for item in object_list.keys():
if object_list[item].height[1] == 0:
#if abs(object_list[item].rgb_area - 3*obj_features_mean.rgb_area.book)/obj_features_std.rgb_area.book > 1:
#print "TOO SMALL/BIG TO BE BOOK"
#del object_list[item]
if object_list[item].name != 'cd':
print "TO BE DELETED: ", item
del object_list[item]
if len(object_list) == 0:
"THERE IS NOTHING INSIDE!!!!!!!!!!!"
pick_obj = object_list.values()[0]
print "==========================================="
print " OBJECT IS: ", pick_obj.name
print "==========================================="
object_df = create_object_df()
create_csv("testing_object_features", object_df)
for item in object_list.keys():
object_dict = prepare_object_dict(object_list[item], object_df)
object_df = object_df.append(object_dict, ignore_index=True)
add_to_csv("testing_object_features.csv", object_df)
########### Find Circles for Pix3world transformation ################
circles = depth_cali[4]
cali_circles_init = circles - circles[0][0]
cali_circles = []
for circ in cali_circles_init[0]:
cali_circles.append([circ[0] / 2, circ[1] / 2])
print cali_circles
p1, inverse = vc.pix3world_cal(cali_circles[0], cali_circles[2],
cali_circles[1])
################ Find the suction or grasping points in pixels ###########
ipt = object_ipt_dict[pick_obj.name][0]
if ipt > 5:
print "OBJECT TO BE PICKED BY GRASPING"
first_node, node1, node2 = gp.first_grasping_point(pick_obj)
#imgimg = gp.display_grasping_points(test_rgb_img, first_node, node1, node2[0], pick_obj, show=True)
print "FIRST NODES: ", first_node, node1, node2
current_line = gp.find_perpendicular_line(node1, node2[0])
if ipt == 10:
perp_vect = np.array(first_node - pick_obj.centre)
print "PERPERP", perp_vect
current_line = [perp_vect[0], perp_vect[1]]
possible_pairs = gp.find_possible_cross_pairs(
pick_obj, first_node, current_line)
#print "Initial Possible Pairs: ", possible_pairs
possible_pairs = gp.remove_duplicates(possible_pairs, node1,
node2[0])
print "CURRENT_LINE: ", current_line
print "POSSIBLE_PAIRS: ", possible_pairs
possible_second_node, possible_grasp_centre = gp.find_second_grasping_point(
possible_pairs, first_node, pick_obj)
print "POSSIBLE_SECOND_NODE AND GRASP_CENTRE: "
print possible_second_node
print possible_grasp_centre
for snode_id, snode in enumerate(
list(reversed(possible_second_node))):
if np.array_equal(snode, first_node):
print snode
possible_second_node.pop(-(snode_id + 1))
possible_grasp_centre.pop(-(snode_id + 1))
second_node, grasp_centre = gp.determine_best_grasping_point(
possible_second_node, possible_grasp_centre, first_node)
print "SECOND NODE: ", second_node
if ipt == 7:
first_node, second_node = gp.fix_torch_orientation(
pick_obj, rgb_normclean, first_node, second_node)
grasp_img = gp.display_grasping_points(test_rgb_img,
first_node,
second_node,
grasp_centre,
pick_obj,
show=SHOW)
cv2.imwrite("display_grasp_img.jpg", grasp_img)
p_cc = [first_node[0], first_node[1]]
X, Y = vc.pix3world(p1, inverse, p_cc)
cc = X, Y
p_ce = [second_node[0], second_node[1]]
X, Y = vc.pix3world(p1, inverse, p_ce)
ce = X, Y
X, Y, Z, ori, aoa, Size = og.get_grasping_coords(cc, ce, 25)
else:
print "OBJECT TO BE PICKED BY SUCTION"
if object_ipt_dict[pick_obj.name][1] == 'rgb':
x_pix = pick_obj.rgb_centre[0]
y_pix = pick_obj.rgb_centre[1]
else:
x_pix = pick_obj.centre[0]
y_pix = pick_obj.centre[1]
p = [x_pix, y_pix]
x, y = vc.pix3world(p1, inverse, p)
x = x[0, 0]
y = y[0, 0]
show_img = copy.copy(test_rgb_img)
cv2.circle(show_img, (int(x_pix), int(y_pix)), 3, (0, 0, 255),
1)
cv2.circle(show_img, (int(x_pix), int(y_pix)), 2, (0, 0, 255),
1)
if SHOW:
plt.figure("Circles")
plt.imshow(show_img)
plt.show()
cv2.imwrite("display_suction_img.jpg", show_img)
print "~~~~~~~~~~~~~~ OBJECT ATTRIBUTES ~~~~~~~~~~~~~~~"
if ipt > 5:
print "GRASPING"
else:
print "SUCTION"
print "Height: ", pick_obj.height
print "RGB Aspect: ", pick_obj.rgb_aspect
try:
print "Circularness: ", pick_obj.circularness
except:
print "no depth"
copy_img = copy.copy((normclean * 255).astype('uint8'))
copy_img = cv2.cvtColor(copy_img, cv2.COLOR_GRAY2RGB)
if sorted_family is not None:
for member in sorted_family:
cv2.drawContours(copy_img, [member['contour'][0]], -1,
(5, 205, 5), 1)
cv2.imwrite("display_depth_profile.jpg", copy_img)
gp.display_second_screen(normclean, sorted_family, object_list,
cost_list, ipt, pick_obj)
#plt.show(block=False)
while (1):
continue_check = raw_input("Continue?: ")
if continue_check != "no":
break
#ipt = int(raw_input("object 1-10: "))
#x = float(raw_input("x :"))
#y = float(raw_input("y :"))
ic.led_serial_send(ser_led, "I", 2, 0, 0, 0)
clr = copy.deepcopy(uw.empty_led)
#ipt = int(raw_input("object 1-10: "))
#x = float(raw_input("x :"))
#y = float(raw_input("y :"))
if ipt == 1: #cd
clr[0] = [255, 255, 0]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.vac_stow(c,
ser_ee,
ser_vac,
ser_led,
x + 35,
y,
0,
z=50)
msg = og.vac_pick(c, ser_ee, ser_vac, ser_led, sy, sz, 4, x=sx)
if ipt == 2: #book
clr[4] = [255, 0, 0]
clr[5] = [255, 0, 0]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.vac_stow(c,
ser_ee,
ser_vac,
ser_led,
x,
y,
4,
z=60,
yoff=-100)
msg = og.vac_pick(c, ser_ee, ser_vac, ser_led, sy, sz, 4, x=sx)
if ipt == 3: #erasor
clr[1] = [0, 0, 255]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.vac_stow(c,
ser_ee,
ser_vac,
ser_led,
x,
y,
1,
z=80,
yoff=-21)
msg = og.vac_pick(c, ser_ee, ser_vac, ser_led, sy, sz, 4, x=sx)
if ipt == 4: #tape_measure
clr[2] = [0, 255, 0]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.vac_stow(c,
ser_ee,
ser_vac,
ser_led,
x,
y,
2,
z=90,
yoff=-21)
msg = og.vac_pick(c, ser_ee, ser_vac, ser_led, sy, sz, 4, x=sx)
if ipt == 5: #box
clr[3] = [255, 0, 255]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.vac_stow(c,
ser_ee,
ser_vac,
ser_led,
x,
y,
3,
z=110,
yoff=-21)
msg = og.vac_pick(c, ser_ee, ser_vac, ser_led, sy, sz, 4, x=sx)
elif ipt == 6: #mug
clr[0] = [255, 64, 0]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.grab_stow(c,
ser_ee,
ser_vac,
ser_led,
X,
Y,
z=20,
angle_of_attack=89.9,
orientation=ori,
shelf=0,
size=6,
xoff=0)
msg = og.grab_pick(c,
ser_ee,
ser_vac,
ser_led,
sy - 20,
z=sz,
orientation=0,
object_height=65.0,
xoff=10,
zoff=2,
n=2)
elif ipt == 7: #torch
clr[1] = [0, 255, 255]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.grab_stow(c,
ser_ee,
ser_vac,
ser_led,
X,
Y,
z=1,
angle_of_attack=89.9,
orientation=ori,
shelf=1,
size=12,
xoff=0,
zoff=0,
obj=ipt)
msg = og.grab_pick(c,
ser_ee,
ser_vac,
ser_led,
sy,
z=sz,
orientation=0,
object_height=17.0,
xoff=2,
zoff=0,
n=2,
obj=ipt,
stored_x=sx)
elif ipt == 8: #duct_tape
clr[2] = [64, 255, 0]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.grab_stow(c,
ser_ee,
ser_vac,
ser_led,
X,
Y,
z=15,
angle_of_attack=89.9,
orientation=ori,
shelf=2,
size=20)
msg = og.grab_pick(c,
ser_ee,
ser_vac,
ser_led,
sy - 32,
z=sz,
orientation=0,
object_height=48.0,
xoff=10,
n=2)
elif ipt == 9: #banana
clr[3] = [64, 64, 255]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.grab_stow(c,
ser_ee,
ser_vac,
ser_led,
X,
Y,
z=8,
angle_of_attack=89.9,
orientation=ori,
shelf=3,
size=28,
xoff=20,
zoff=-30)
msg = og.grab_pick(c,
ser_ee,
ser_vac,
ser_led,
sy - 17,
z=sz,
orientation=0,
object_height=33.0,
xoff=27,
n=2)
elif ipt == 10: #tennis_ball
clr[4] = [255, 0, 64]
clr[5] = [255, 0, 64]
lf.illuminate_cluster(ser_led, 1, colour=clr)
msg, sx, sy, sz = og.grab_stow(c,
ser_ee,
ser_vac,
ser_led,
X,
Y,
z=20,
angle_of_attack=89.9,
orientation=ori,
shelf=4,
size=50,
zoff=-45,
obj=ipt)
pt = raw_input("continue?")
msg = og.grab_pick(c,
ser_ee,
ser_vac,
ser_led,
sy,
z=sz,
orientation=0,
object_height=65.0,
n=2,
obj=ipt,
stored_x=sx)
clr = copy.deepcopy(uw.empty_led)
lf.illuminate_cluster(ser_led, 1, colour=clr)
ic.led_serial_send(ser_led, "I", 2, 0, 2, 0)
if task == "dg":
while True:
demand_Pose = {
"x": -200,
"y": -400.0,
"z": random.uniform(20, 150),
"rx": 0.0,
"ry": 180.0,
"rz": 0.0
}
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=4)
current_Pose = ic.get_ur_position(c, 1)
for i in range(0, 3):
if current_Pose[i] == 0:
ipt = raw_input("Continue?")
if task == "tr":
success_rate = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
while True:
#initialize_waypoints()
ipt = int(raw_input("object 1-10: "))
if ipt < 6:
msg = og.vac_stow(c, ser_ee, ser_vac, -300, -400, 1)
elif ipt == 6:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=20,
angle_of_attack=89.9,
shelf=1,
size=6)
elif ipt == 7:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=6,
angle_of_attack=89.9,
shelf=1,
size=12)
elif ipt == 8:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=15,
angle_of_attack=89.9,
shelf=1,
size=20)
elif ipt == 9:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=8,
angle_of_attack=89.9,
shelf=1,
size=25)
elif ipt == 10:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=20,
angle_of_attack=89.9,
shelf=1,
size=50)
ipt2 = int(raw_input("fail/success (0/1): "))
success_rate[0][ipt - 1] = success_rate[0][ipt - 1] + 1
if ipt2 == 1:
success_rate[1][ipt - 1] = success_rate[1][ipt - 1] + 1
for i in range(0, 10):
if success_rate[0][i] != 0:
print "object ", i + 1, ": ", float(
success_rate[1][i]), " ot of ", float(
success_rate[0][i])
if task == "clean":
nc.clean_board(c, ser_ee, ser_vac)
if task == "tally":
nc_win = int(raw_input("win: "))
nc_loss = int(raw_input("loss: "))
nc_draw = int(raw_input("draw: "))
nc.update_tally(c,
ser_ee,
ser_vac,
win=nc_win,
loss=nc_loss,
draw=nc_draw)
if task == "gc":
p = [0, 0]
p[0] = float(raw_input("pc_x :"))
p[1] = float(raw_input("pc_y :"))
X, Y = cal_test(p)
cc = [X, Y]
p[0] = float(raw_input("pe_x :"))
p[1] = float(raw_input("pe_y :"))
X, Y = cal_test(p)
ce = [X, Y]
X, Y, Z, ori, aoa, Size = get_grasping_coords(cc, ce, 25)
print X, Y, Z, ori, aoa, Size
print grab_stow(c,
ser_ee,
ser_vac,
X,
Y,
z=Z,
orientation=ori,
angle_of_attack=aoa,
shelf=0,
size=Size)
print X, Y, Z, ori, aoa, Size
if task == "nc":
nc_win = int(raw_input("robot wins: "))
nc_draw = int(raw_input("draws: "))
nc_loss = int(raw_input("human wins: "))
while True:
st = int(raw_input("Robot/Human starts (0/1): "))
game = nc.naughts_crosses(c, ser_ee, ser_vac, start=st)
print game
for i in range(0, 5):
current_Pose, current_Grip = ic.get_position(c,
ser_ee,
ser_vac,
CMD=1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2] - 10,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
CMD=4)
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]
}
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
CMD=4)
if game == "................Robot Victory!................":
nc_win = nc_win + 1
nc.update_tally(c, ser_ee, ser_vac, win=nc_win)
elif game == ".................Human victory................":
nc_loss = nc_loss + 1
nc.update_tally(c, ser_ee, ser_vac, loss=nc_loss)
elif game == ".....................Draw.....................":
nc_draw = nc_draw + 1
nc.update_tally(c, ser_ee, ser_vac, draw=nc_draw)
if task == "cam":
check_camera()
if task == "grab_pick":
#shelf = int(raw_input("shelf: "))
#ori = float(raw_input("ori: "))
y = float(raw_input("y: "))
Z = float(raw_input("z: "))
height = int(raw_input("height: "))
print og.grab_pick(c,
ser_ee,
ser_vac,
y,
z=Z,
orientation=0,
object_height=height)
if task == "demo":
demand_Grip = dict(uw.end_effector_home)
while True:
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["tilt"] = 1
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["act"] = 30
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["servo"] = 0
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["vac"] = "g"
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["servo"] = 80
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["tilt"] = 0
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["vac"] = "r"
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["act"] = 50
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
if task == "vac_stow":
x = float(raw_input("x: "))
y = float(raw_input("y: "))
shelf = int(raw_input("shelf: "))
print ic.vac_stow(c, ser_ee, ser_vac, x, y, shelf)
if task == "vac_pick":
y = float(raw_input("y: "))
z = float(raw_input("z: "))
shelf = int(raw_input("shelf: "))
print og.vac_pick(c, ser_ee, ser_vac, y, z, shelf)
if task == "shelf":
shelf = int(raw_input("shelf: "))
#print ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(uw.shelf_joints_waypoint),CMD=2)
print ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.shelf_joints[shelf]),
CMD=2)
#print ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(uw.shelf_joints_waypoint),CMD=2)
#print ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(shelf_home_joints),CMD=2)
if task == "shelf_home":
print ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.shelf_joints_waypoint),
CMD=2)
if task == "home":
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
CMD=2)
if task == "force":
print ic.get_force(c)
if task == "torque":
print ic.get_torque(c)
if task == "pose":
current_Pose, current_Grip = ic.get_position(c,
ser_ee,
ser_vac,
CMD=1)
print "current pose: ", current_Pose
print "current grip: ", current_Grip
if task == "joints":
current_Joints, current_Grip = ic.get_position(c,
ser_ee,
ser_vac,
CMD=3)
print "current joints: ", current_Joints
print "current grip: ", current_Grip
if task == "move":
demand_Pose["x"] = float(raw_input("x: "))
demand_Pose["y"] = float(raw_input("y: "))
demand_Pose["z"] = float(raw_input("z: "))
demand_Pose["rx"] = float(raw_input("rx: "))
demand_Pose["ry"] = float(raw_input("ry: "))
demand_Pose["rz"] = float(raw_input("rz: "))
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
CMD=4)
if task == "grab":
demand_Grip = dict(uw.end_effector_home)
demand_Grip["act"] = int(raw_input("act: "))
demand_Grip["servo"] = int(raw_input("servo: "))
demand_Grip["tilt"] = int(raw_input("tilt: "))
demand_Grip["vac"] = raw_input("vac: ")
msg = ic.safe_move(c, ser_ee, ser_vac, Grip=demand_Grip, CMD=0)
def main():
c, ser_ee, ser_vac, ser_led = initialize()
time.sleep(2)
print "Ready"
# loop
print c.recv(1024)
inp = raw_input("Continue?")
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.og.grab_home_joints),
CMD=2)
while True:
task = raw_input("task: ")
if task == "lf":
lf.illuminate_cluster(ser_led,
3,
colour=[[255, 0, 0], [127, 127, 0],
[0, 255, 0], [0, 127, 127],
[0, 0, 255], [127, 0, 127]])
if task == "ls":
shelf = int(raw_input("cluster: "))
ic.led_serial_send(ser_led, "C", shelf, 255, 255, 255)
for i in range(0, 6):
if i != shelf:
ic.led_serial_send(ser_led, "C", i, 0, 0, 0)
ic.led_serial_send(ser_led, "S", 1, 0, 0, 0)
if task == "led":
#led_serial_send(ser_led,"I",3,127,0,0)
cmd = raw_input("cmd: ")
shelf = int(raw_input("cluster: "))
r = int(raw_input("r: "))
g = int(raw_input("g: "))
b = int(raw_input("b: "))
ic.led_serial_send(ser_led, cmd, shelf, r, g, b)
#while True:
# print ser_led.readline()
if task == "gp":
while True:
ipt = int(raw_input("object 1-10: "))
x = float(raw_input("x :"))
y = float(raw_input("y :"))
if ipt == 1: #cd
msg = og.vac_stow(c, ser_ee, ser_vac, x + 35, y, 1)
msg = og.vac_pick(c, ser_ee, ser_vac, -100, 300, 2)
if ipt == 2: #book
msg = og.vac_stow(c, ser_ee, ser_vac, x, y, 4)
msg = og.vac_pick(c, ser_ee, ser_vac, -500, 100, 2)
if ipt == 3: #erasor
msg = og.vac_stow(c, ser_ee, ser_vac, x, y, 1)
msg = og.vac_pick(c, ser_ee, ser_vac, -100, 300, 2)
if ipt == 4: #tape_measure
msg = og.vac_stow(c, ser_ee, ser_vac, x, y, 1)
msg = og.vac_pick(c, ser_ee, ser_vac, -100, 300, 2)
if ipt == 5: #box
msg = og.vac_stow(c, ser_ee, ser_vac, x, y, 1)
msg = og.vac_pick(c, ser_ee, ser_vac, -100, 300, 2)
elif ipt == 6: #mug
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=20,
angle_of_attack=89.9,
shelf=1,
size=6)
msg = og.grab_pick(c,
ser_ee,
ser_vac,
-320,
z=300,
orientation=0,
object_height=48)
elif ipt == 7: #torch
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=6,
angle_of_attack=89.9,
shelf=1,
size=12)
elif ipt == 8: #duct_tape
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=15,
angle_of_attack=89.9,
shelf=1,
size=20)
elif ipt == 9: #banana
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=8,
angle_of_attack=89.9,
shelf=1,
size=25)
elif ipt == 10: #tennis_ball
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=20,
angle_of_attack=89.9,
shelf=1,
size=50)
if task == "dg":
while True:
demand_Pose = {
"x": -200,
"y": -400.0,
"z": random.uniform(20, 150),
"rx": 0.0,
"ry": 180.0,
"rz": 0.0
}
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=4)
current_Pose = get_ur_position(c, 1)
for i in range(0, 3):
if current_Pose[i] == 0:
ipt = raw_input("Continue?")
if task == "tr":
success_rate = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
while True:
#initialize_waypoints()
ipt = int(raw_input("object 1-10: "))
if ipt < 6:
msg = og.vac_stow(c, ser_ee, ser_vac, -300, -400, 1)
elif ipt == 6:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=20,
angle_of_attack=89.9,
shelf=1,
size=6)
elif ipt == 7:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=6,
angle_of_attack=89.9,
shelf=1,
size=12)
elif ipt == 8:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=15,
angle_of_attack=89.9,
shelf=1,
size=20)
elif ipt == 9:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=8,
angle_of_attack=89.9,
shelf=1,
size=25)
elif ipt == 10:
msg = og.grab_stow(c,
ser_ee,
ser_vac,
-200,
-400,
z=20,
angle_of_attack=89.9,
shelf=1,
size=50)
ipt2 = int(raw_input("fail/success (0/1): "))
success_rate[0][ipt - 1] = success_rate[0][ipt - 1] + 1
if ipt2 == 1:
success_rate[1][ipt - 1] = success_rate[1][ipt - 1] + 1
for i in range(0, 10):
if success_rate[0][i] != 0:
print "object ", i + 1, ": ", float(
success_rate[1][i]), " ot of ", float(
success_rate[0][i])
if task == "clean":
nc.clean_board(c, ser_ee, ser_vac)
if task == "tally":
nc_win = int(raw_input("win: "))
nc_loss = int(raw_input("loss: "))
nc_draw = int(raw_input("draw: "))
nc.update_tally(c,
ser_ee,
ser_vac,
win=nc_win,
loss=nc_loss,
draw=nc_draw)
if task == "gc":
p = [0, 0]
p[0] = float(raw_input("pc_x :"))
p[1] = float(raw_input("pc_y :"))
X, Y = demos.cal_test(p)
cc = [X, Y]
p[0] = float(raw_input("pe_x :"))
p[1] = float(raw_input("pe_y :"))
X, Y = demos.cal_test(p)
ce = [X, Y]
X, Y, Z, ori, aoa, Size = og.get_grasping_coords(cc, ce, 25)
print X, Y, Z, ori, aoa, Size
print og.grab_stow(c,
ser_ee,
ser_vac,
X,
Y,
z=Z,
orientation=ori,
angle_of_attack=aoa,
shelf=0,
size=Size)
print X, Y, Z, ori, aoa, Size
if task == "nc":
nc_win = int(raw_input("robot wins: "))
nc_draw = int(raw_input("draws: "))
nc_loss = int(raw_input("human wins: "))
while True:
st = int(raw_input("Robot/Human starts (0/1): "))
game = nc.naughts_crosses(c, ser_ee, ser_vac, start=st)
print game
for i in range(0, 5):
current_Pose, current_Grip = ic.get_position(c,
ser_ee,
ser_vac,
CMD=1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2] - 10,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
CMD=4)
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]
}
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
CMD=4)
if game == "................Robot Victory!................":
nc_win = nc_win + 1
nc.update_tally(c, ser_ee, ser_vac, win=nc_win)
elif game == ".................Human victory................":
nc_loss = nc_loss + 1
nc.update_tally(c, ser_ee, ser_vac, loss=nc_loss)
elif game == ".....................Draw.....................":
nc_draw = nc_draw + 1
nc.update_tally(c, ser_ee, ser_vac, draw=nc_draw)
if task == "cam":
vc.check_camera()
if task == "grab_pick":
#shelf = int(raw_input("shelf: "))
#ori = float(raw_input("ori: "))
y = float(raw_input("y: "))
Z = float(raw_input("z: "))
height = int(raw_input("height: "))
print og.grab_pick(c,
ser_ee,
ser_vac,
y,
z=Z,
orientation=0,
object_height=height)
if task == "demo":
demand_Grip = dict(uw.end_effector_home)
while True:
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["tilt"] = 1
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["act"] = 30
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["servo"] = 0
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["vac"] = "g"
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["servo"] = 80
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["tilt"] = 0
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["vac"] = "r"
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["act"] = 50
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.grab_home_joints),
Grip=demand_Grip,
CMD=2)
if task == "vac_stow":
x = float(raw_input("x: "))
y = float(raw_input("y: "))
shelf = int(raw_input("shelf: "))
print og.vac_stow(c, ser_ee, ser_vac, x, y, shelf)
if task == "vac_pick":
y = float(raw_input("y: "))
z = float(raw_input("z: "))
shelf = int(raw_input("shelf: "))
print og.vac_pick(c, ser_ee, ser_vac, y, z, shelf)
if task == "shelf":
shelf = int(raw_input("shelf: "))
#print ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(shelf_joints_waypoint),CMD=2)
print ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.shelf_joints[shelf]),
CMD=2)
#print ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(shelf_joints_waypoint),CMD=2)
#print ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(shelf_home_joints),CMD=2)
if task == "shelf_home":
print ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.shelf_joints_waypoint),
CMD=2)
if task == "home":
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.og.grab_home_joints),
CMD=2)
if task == "force":
print get_force(c)
if task == "torque":
print get_torque(c)
if task == "pose":
current_Pose, current_Grip = ic.get_position(c,
ser_ee,
ser_vac,
CMD=1)
print "current pose: ", current_Pose
print "current grip: ", current_Grip
if task == "joints":
current_Joints, current_Grip = ic.get_position(c,
ser_ee,
ser_vac,
CMD=3)
print "current joints: ", current_Joints
print "current grip: ", current_Grip
if task == "move":
demand_Pose["x"] = float(raw_input("x: "))
demand_Pose["y"] = float(raw_input("y: "))
demand_Pose["z"] = float(raw_input("z: "))
demand_Pose["rx"] = float(raw_input("rx: "))
demand_Pose["ry"] = float(raw_input("ry: "))
demand_Pose["rz"] = float(raw_input("rz: "))
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
CMD=4)
if task == "grab":
demand_Grip = dict(uw.end_effector_home)
demand_Grip["act"] = int(raw_input("act: "))
demand_Grip["servo"] = int(raw_input("servo: "))
demand_Grip["tilt"] = int(raw_input("tilt: "))
demand_Grip["vac"] = raw_input("vac: ")
msg = ic.safe_move(c, ser_ee, ser_vac, Grip=demand_Grip, CMD=0)