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_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 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 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"