def z_cal(c, ser_ee, ser_vac):
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": 30,
"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)
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": 0,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
object_height = 1000.0 * float(
ic.safe_ur_move_only(c, ser_ee, ser_vac, Pose=demand_Pose, CMD=5))
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
return object_height + 6
def banana(c, ser_ee, ser_vac, x, y, rz):
# banana picking strategy
demand_Pose = copy.deepcopy(grab_home)
demand_Grip = copy.deepcopy(end_effector_home)
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grab_home_joints),
Grip=demand_Grip,
CMD=2)
demand_Grip["act"] = 40
current_Joints, current_Grip = get_position(c, ser_ee, ser_vac, CMD=3)
demand_Joints = {
"x": current_Joints[0],
"y": current_Joints[1],
"z": current_Joints[2],
"rx": current_Joints[3],
"ry": current_Joints[4],
"rz": current_Joints[5] + rz
}
ur_move(c, ser_ee, ser_vac, Pose=demand_Joints, Grip=demand_Grip, CMD=2)
time.sleep(1)
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": x,
"y": current_Pose[1],
"z": current_Pose[2],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
ur_move(c, ser_ee, ser_vac, Pose=demand_Pose, Grip=demand_Grip)
demand_Pose["y"] = y
ur_move(c, ser_ee, ser_vac, Pose=demand_Pose, Grip=demand_Grip)
demand_Pose["z"] = 8
ur_move(c, ser_ee, ser_vac, Pose=demand_Pose, Grip=demand_Grip)
demand_Grip["servo"] = 0
ur_move(c, ser_ee, ser_vac, Pose=demand_Pose, Grip=demand_Grip)
time.sleep(0.5)
demand_Grip["vac"] = "g"
ur_move(c, ser_ee, ser_vac, Pose=demand_Pose, Grip=demand_Grip)
time.sleep(2)
demand_Pose["z"] = 100
ur_move(c, ser_ee, ser_vac, Pose=demand_Pose, Grip=demand_Grip)
ur_move(c, ser_ee, ser_vac)
demand_Grip["servo"] = 80
demand_Grip["vac"] = "r"
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grab_home_joints),
Grip=demand_Grip,
CMD=2)
return
def update_tally(c, ser_ee, ser_vac, win=0, loss=0, draw=0):
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(tally_home_joints),
CMD=2)
dx = 0
dz = 0
board_offset = -160
if win != 0:
dz = (win - 1) / 5
dx = win % 5
elif loss != 0:
dz = (loss - 1) / 5
dx = loss % 5
board_offset = 100
elif draw != 0:
dz = (draw - 1) / 5
dx = draw % 5
board_offset = -30
current_Pose, current_Grip = ic.get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": current_Pose[0] + board_offset + 15 * dx,
"y": current_Pose[1],
"z": current_Pose[2] - 60 * dz,
"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["y"] = current_Pose[1] + 20
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=4)
if dx == 0:
demand_Pose["x"] = current_Pose[0] + board_offset + 15 * 5
demand_Pose["z"] = current_Pose[2] - 60 * dz - 40
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=8)
demand_Pose["y"] = current_Pose[1]
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=4)
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(tally_home_joints),
CMD=2)
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
def smooth_rotate(c,ser_ee,ser_vac,orientation=0,angle_of_attack=0):
# Select tcp_2, for rotations around the grasping point
ic.socket_send(c,sCMD=101)
# 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 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]
# Create rotation matrix for current position
R=z_rot*y_rot*x_rot
# 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, 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],"rx":rx,"ry":ry,"rz":rz}
msg = ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Speed=0.15,CMD=8)
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 clean_board(c, ser_ee, ser_vac):
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
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": 30,
"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)
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": 0,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
object_height = 1000.0 * float(
ic.safe_ur_move(c, ser_ee, ser_vac, Pose=demand_Pose, CMD=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],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
demand_Grip = {"act": 30, "servo": 80, "tilt": 0, "vac": 'r'}
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Grip=demand_Grip,
CMD=4)
demand_Grip["servo"] = 0
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Grip=demand_Grip,
CMD=4)
# Adjust actuator position
demand_Grip["act"] = 40
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)
# Close grabber servo
demand_Grip["servo"]=0
msg = ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
time.sleep(0.2)
'''
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": 40,
"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_Grip["act"] = 40
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Grip=demand_Grip,
CMD=4)
# Rotate around y axis by orientation
# Convert to radians
angle_of_attack = 89.9 * math.pi / 180.0
orientation = 15.0 * math.pi / 180.0
thetay = 135.0 * math.pi / 180.0
# Cartesian rotation matrices to match 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]
# Create rotation matrix for neutral position
R = z_rot * y_rot * x_rot
# Axis rotation matricies for pointing down then rotate around y-axis, rotate around x-axis by 89.9, then y-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]
y_rot = np.matrix([[math.cos(orientation), 0.0, -math.sin(orientation)],
[0.0, 1.0, 0.0],
[math.sin(orientation), 0.0,
math.cos(orientation)]]) # y_rot[rows][columns]
# Cartesian rotation matrix of desired orientation
R = y_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
ic.socket_send(c, sCMD=101)
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],
"rx": rx,
"ry": ry,
"rz": rz
}
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=8)
ic.socket_send(c, sCMD=102)
# Clean board
current_Pose, current_Grip = ic.get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": current_Pose[0] + 100,
"y": current_Pose[1] - 50,
"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), CMD=4)
demand_Pose["z"] = object_height + 10
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=4)
demand_Pose["x"] = current_Pose[0] + 250
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=8)
demand_Pose["x"] = current_Pose[0] + 100
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=8)
demand_Pose["y"] = current_Pose[1] + 50
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=8)
demand_Pose["x"] = current_Pose[0] + 250
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=8)
demand_Pose["x"] = current_Pose[0] + 100
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=8)
demand_Pose["z"] = 50
msg = ic.safe_ur_move(c, ser_ee, ser_vac, Pose=dict(demand_Pose), CMD=4)
# Return erasor
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
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": 30,
"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": object_height + 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=8)
demand_Grip["act"] = 30
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Grip=demand_Grip,
CMD=4)
demand_Grip["act"] = 30
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_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Grip=demand_Grip,
CMD=4)
time.sleep(0.2)
# Adjust actuator position
demand_Grip["act"] = 80
msg = ic.safe_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
Grip=demand_Grip,
CMD=2)
def nc_robot_move(c, ser_ee, ser_vac, square, z):
#pick square
msg = ic.socket_send(c, sCMD=102)
X_CENTRE = 50.0
Y_CENTRE = -405.0
GRID_WIDTH = 50
GRID_HEIGHT = 50
x = X_CENTRE + (1 - square / 3) * GRID_HEIGHT
y = Y_CENTRE + (1 - square % 3) * GRID_WIDTH
# Cartesian rotation matrices to match grabbing_joints rotation
x_rot = np.matrix(
[[1.0, 0.0, 0.0],
[0.0, math.cos(math.pi - 0.001), -math.sin(math.pi - 0.001)],
[0.0, math.sin(math.pi - 0.001),
math.cos(math.pi - 0.001)]]) # x_rot[rows][columns]
y_rot = np.matrix([[math.cos(0.0), 0.0, -math.sin(0.0)], [0.0, 1.0, 0.0],
[math.sin(0.0), 0.0,
math.cos(0.0)]]) # y_rot[rows][columns]
z_rot = np.matrix([[math.cos(math.pi / 4), -math.sin(math.pi / 4), 0.0],
[math.sin(math.pi / 4),
math.cos(math.pi / 4), 0.0],
[0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Create rotation matrix for current position
R = z_rot * y_rot * x_rot
# 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
#inp = raw_input("Continue?")
# Move to drawing waypoint
msg = ic.safe_ur_move_only(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
current_Pose, current_Grip = ic.get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz": rz}
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Speed=1.5,
CMD=8)
# 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]
'''for i in range(0,2):
z_rot = np.matrix([[ math.cos(math.pi/2), -math.sin(math.pi/2), 0.0],
[ math.sin(math.pi/2), math.cos(math.pi/2), 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
#inp = raw_input("Continue?")
# Rotate around tool centre point defined by tcp_2
demand_Pose = {"x":x,"y":y,"z":z,"rx":rx,"ry":ry,"rz":rz}
msg = ic.safe_ur_move(c,ser_ee,ser_vac,Pose=dict(demand_Pose),CMD=8)
'''
for i in range(0, 4):
z_rot = np.matrix(
[[math.cos(-math.pi / 2), -math.sin(-math.pi / 2), 0.0],
[math.sin(-math.pi / 2),
math.cos(-math.pi / 2), 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
#inp = raw_input("Continue?")
# Rotate around tool centre point defined by tcp_2
demand_Pose = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz": rz}
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(demand_Pose),
Speed=1.5,
CMD=8)
msg = ic.socket_send(c, sCMD=100)
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
def naughts_crosses(c, ser_ee, ser_vac, start=0):
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
z_height = z_cal(c, ser_ee, ser_vac)
print z_height
#ipt = raw_input("Continue?")
time.sleep(0.2)
empty_grid, top_left, bottom_right = cal_grid_im("empty.jpg")
grid_state = [0, 0, 0, 0, 0, 0, 0, 0, 0]
if start == 0:
#draw
#set grid_state
grid_state, sq = nc_pick_move(dict(grid_state))
nc_robot_move(c, ser_ee, ser_vac, sq, z_height)
print "completed move"
print grid_state[0:3]
print grid_state[3:6]
print grid_state[6:9]
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
time.sleep(0.2)
grid_ref = get_grid_im("grid_ref.jpg", top_left, bottom_right)
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)
while min(grid_state) == 0:
# Wait for player move
print "waiting for player move"
grid_state = get_nc_state(empty_grid, grid_ref, top_left, bottom_right,
dict(grid_state))
print grid_state[0:3]
print grid_state[3:6]
print grid_state[6:9]
# Check game state
win = nc_finish(dict(grid_state))
if win == 1:
return "................Robot Victory!................"
if win == 2:
return ".................Human victory................"
if min(grid_state) != 0:
break
#draw
#set grid_state
grid_state, sq = nc_pick_move(dict(grid_state))
nc_robot_move(c, ser_ee, ser_vac, sq, z_height)
print "completed move"
print grid_state[0:3]
print grid_state[3:6]
print grid_state[6:9]
msg = ic.safe_ur_move(c,
ser_ee,
ser_vac,
Pose=dict(uw.naughts_crosses_cam_joints),
CMD=2)
time.sleep(0.2)
grid_ref = get_grid_im("grid_ref.jpg", top_left, bottom_right)
# Check game state
win = nc_finish(dict(grid_state))
if win == 1:
return "Robot Victory!"
if win == 2:
return "fleshbag victory"
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)
return ".....................Draw....................."
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 grab_pick(c,
ser_ee,
ser_vac,
y,
z=12,
orientation=0,
object_height=30,
size=70):
# curved object picking strategy
#socket_send(c,sCMD=101)
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
'''
demand_Pose = copy.deepcopy(grab_home)
demand_Grip = copy.deepcopy(end_effector_home)
demand_Grip["act"]=size
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(shelf_home_joints),Grip=demand_Grip,CMD=2)
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(shelf_joints_waypoint),Grip=demand_Grip,CMD=2)
demand_Joints = copy.deepcopy(shelf_joints[shelf])
demand_Joints["rz"] = demand_Joints["rz"]+orientation
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(demand_Joints),Grip=demand_Grip,CMD=2)
current_Pose, current_Grip = get_position(c,ser_ee,ser_vac,CMD=1)
demand_Pose = {"x":800,"y":y,"z":current_Pose[2]+object_height,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(demand_Pose),Grip=demand_Grip,Speed=0.5,CMD=4)
demand_Grip["servo"]=0
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(demand_Pose),Grip=demand_Grip,Speed=0.5,CMD=4)
time.sleep(0.5)
demand_Pose["z"]=demand_Pose["z"]+10
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(demand_Pose),Grip=demand_Grip,Speed=0.5,CMD=4)
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(demand_Joints),Grip=demand_Grip,CMD=2)
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(shelf_home_joints),Grip=demand_Grip,CMD=2)
demand_Grip["servo"]=80
msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(shelf_home_joints),Grip=demand_Grip,CMD=2)
#socket_send(c,sCMD=100)
'''
demand_Grip = copy.deepcopy(end_effector_home)
demand_Grip["act"] = int(80 - 0.8 * object_height)
ser_ee.flush
print "Sending actuator move"
ser_ee.write("A" + chr(demand_Grip["act"]) + "\n")
#demand_Grip["servo"] = 0
msg = safe_ur_move_only(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_grab_joints[shelf]),
CMD=2)
while True:
ipt = ser_ee.readline()
print ipt
if ipt == "done\r\n":
break
timeout = ser_ee.readline()
print "timeout: ", timeout
ser_ee.flush
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": current_Pose[0],
"y": y,
"z": current_Pose[2] + object_height - 37,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
CMD=4)
yoffset = 20.0 * (y + 73.0) / 480.0
demand_Pose["x"] = current_Pose[0] + 160.0 - yoffset
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
Speed=0.25,
CMD=8)
demand_Pose["z"] = current_Pose[2] + object_height - 52
#demand_Grip["servo"]=60
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
Speed=0.2,
CMD=4)
demand_Pose["x"] = current_Pose[0] + 90 - yoffset
demand_Grip["servo"] = 0
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
Speed=0.25,
CMD=8)
demand_Pose["x"] = current_Pose[0] - 30 - yoffset
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
Speed=0.25,
CMD=8)
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints_waypoint),
Grip=demand_Grip,
CMD=2)
return "Completed pick from shelf " + str(shelf)
def grab_stow(c,
ser_ee,
ser_vac,
x,
y,
z=30,
orientation=0,
angle_of_attack=0,
shelf=0,
size=70):
# Select tcp_2, for rotations around the grasping point
socket_send(c, sCMD=101)
object_size = 80 - int(size)
# 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 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
demand_Grip = copy.deepcopy(end_effector_home)
demand_Grip["act"] = object_size - 10
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grabbing_joints_waypoint),
Grip=demand_Grip,
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, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2],
"rx": rx,
"ry": ry,
"rz": rz
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
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, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2],
"rx": rx,
"ry": ry,
"rz": rz
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
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, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2],
"rx": rx,
"ry": ry,
"rz": rz
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
CMD=8)
# Move to above object
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": x,
"y": y,
"z": z + 50,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(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 = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(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]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
CMD=8,
Speed=0.4)
# Grab with reduced chance of collision
# Partially close grabber servo
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Grip = {
"act": current_Grip[0] - 300,
"servo": 30,
"tilt": current_Grip[2] & 0x20,
"vac": current_Grip[4]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
CMD=4)
# Adjust actuator position
demand_Grip["act"] = object_size
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
CMD=4)
# Open grabber servo
#demand_Grip["servo"]=80
#msg = safe_ur_move(c,ser_ee,ser_vac,Pose=copy.deepcopy(demand_Pose),Grip=demand_Grip,CMD=4)
# Close grabber servo
demand_Grip["servo"] = 0
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
CMD=4)
time.sleep(0.5)
# Lift object
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": 200,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Pose),
Grip=demand_Grip,
CMD=4)
# Move safely towards shelf
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grabbing_joints_waypoint),
Grip=demand_Grip,
CMD=2)
# Move safely towards shelf
current_Joints, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
Grip=demand_Grip,
CMD=2)
# Move safely towards shelf
demand_Joints = copy.deepcopy(shelf_joints_waypoint)
demand_Joints["rz"] = demand_Joints["rz"] - 90
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Joints),
Grip=demand_Grip,
CMD=2)
# Move safely towards shelf
demand_Joints = copy.deepcopy(shelf_joints[shelf])
demand_Joints["rz"] = demand_Joints["rz"] - 90
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(demand_Joints),
Grip=demand_Grip,
CMD=2)
# Define position on shelf
object_height = 50
shelf_depth = 788
yoff = 35
# Align object with shelf
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Move into shelf
demand_Pose["x"] = shelf_depth
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Release object
demand_Grip["servo"] = 80
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
time.sleep(1)
# Move back
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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Return home
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints[shelf]),
CMD=2)
# Return home
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints_waypoint),
CMD=2)
# Return home
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grab_home_joints),
CMD=2)
# Reset tool to tcp_1
socket_send(c, sCMD=100)
return "Shelf " + str(shelf) + " completed"
def vac_pick(c, ser_ee, ser_vac, y, z, n):
# Determine closest shelf
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
# 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 = copy.deepcopy(grab_home)
demand_Grip = copy.deepcopy(end_effector_home)
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_home_joints),
Grip=demand_Grip,
CMD=2)
# Move to shelves
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints_waypoint),
Grip=demand_Grip,
CMD=2)
# Move to shelf
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints[shelf]),
Grip=demand_Grip,
CMD=2)
# Align with object
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = copy.deepcopy(demand_Pose)
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Move above object
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": 816.3,
"y": y,
"z": z,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=8)
# Move down until object is reached
demand_Pose = {
"x": 816.3,
"y": y,
"z": z - 50,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
print "sending force_move................................................"
object_height = 1000.0 * float(
safe_ur_move(c,
ser_ee,
ser_vac,
Pose=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, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
time.sleep(2)
# Lift object
demand_Pose = {
"x": 816.3,
"y": y,
"z": z,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Speed=0.25,
Grip=demand_Grip,
CMD=4)
# Exit shelf
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=object_pos,
Speed=0.75,
Grip=demand_Grip,
CMD=4)
# Move away from shelf
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints_waypoint),
Grip=demand_Grip,
CMD=2)
# Rotate base
current_Joints, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
Grip=demand_Grip,
CMD=2)
# Move to new position
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Release object
demand_Grip["vac"] = "r"
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
time.sleep(2)
# Return home
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_home_joints),
Grip=demand_Grip,
CMD=2)
return "Completed pick from shelf " + str(shelf)
def vac_stow(c, ser_ee, ser_vac, x, y, shelf):
# Home
demand_Pose = copy.deepcopy(grab_home)
demand_Grip = copy.deepcopy(end_effector_home)
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grab_home_joints),
Grip=demand_Grip,
CMD=2)
# Rotate end effector
current_Joints, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
Grip=demand_Grip,
CMD=2)
# Avoid camera mount
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Rotate base
current_Joints, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
Grip=demand_Grip,
CMD=2)
# Move to above the object
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": x,
"y": y,
"z": 100,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Move down until oject is reached
demand_Pose = {
"x": x,
"y": y,
"z": 25,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
print "sending force_move................................................"
object_height = 1000.0 * float(
safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Speed=0.05,
Grip=demand_Grip,
CMD=5))
print "object_height: ", object_height
# Turn on vacuum at current position
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Speed=0.25,
Grip=demand_Grip,
CMD=4)
# Rotate base to shelves
current_Joints, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
Grip=demand_Grip,
CMD=2)
# Move closer to shelves
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints_waypoint),
Grip=demand_Grip,
CMD=2)
# Move to specific shelf
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints[shelf]),
Grip=demand_Grip,
CMD=2)
# Set depth on shelf
if object_height < 57:
shelf_depth = 816
elif object_height < 100:
shelf_depth = 788
else:
print "Object too tall"
# Raise end to object_height above the shelf
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2] + object_height,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Move to back of shelf
demand_Pose = {
"x": shelf_depth,
"y": current_Pose[1],
"z": current_Pose[2] + object_height,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Release vacuum at current position
demand_Grip["vac"] = "r"
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
time.sleep(2)
# Exit shelf
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2] + object_height,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Return home
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints[shelf]),
CMD=2)
# Return home
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(shelf_joints_waypoint),
CMD=2)
# Return home
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grab_home_joints),
Grip=demand_Grip,
CMD=2)
print "object_height: ", object_height
return "Shelf " + str(shelf) + " completed"
def fork(c, ser_ee, ser_vac, x, y, rz):
# fork picking strategy
demand_Pose = copy.deepcopy(grab_home)
demand_Grip = copy.deepcopy(end_effector_home)
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grab_home_joints),
Grip=demand_Grip,
CMD=2)
current_Joints, current_Grip = get_position(c, ser_ee, ser_vac, CMD=3)
demand_Joints = {
"x": current_Joints[0] - 90,
"y": current_Joints[1],
"z": current_Joints[2],
"rx": current_Joints[3],
"ry": current_Joints[4] + 60,
"rz": current_Joints[5] - 45
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
Grip=demand_Grip,
CMD=2)
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=1)
demand_Pose = {
"x": x,
"y": y,
"z": 100 + rz,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
demand_Pose = {
"x": x,
"y": y,
"z": 25 + rz,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
print "sending force_move................................................"
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Speed=0.05,
Grip=demand_Grip,
CMD=5)
current_Pose, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
time.sleep(2)
demand_Pose = {
"x": x,
"y": y,
"z": 100 + rz,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Pose,
Speed=0.25,
Grip=demand_Grip,
CMD=4)
current_Joints, current_Grip = get_position(c, ser_ee, ser_vac, CMD=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 = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
Grip=demand_Grip,
CMD=2)
demand_Grip["vac"] = "r"
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=demand_Joints,
Grip=demand_Grip,
CMD=2)
time.sleep(4)
msg = safe_ur_move(c,
ser_ee,
ser_vac,
Pose=copy.deepcopy(grab_home_joints),
Grip=demand_Grip,
CMD=2)
return
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)
