def main():
c, ser_ee = initialize()
# loop
print c.recv(1024)
inp = raw_input("Continue?")
msg = ic.safe_move(c, ser_ee, Pose=dict(iw.home_joints), CMD=2)
cali_img = cv2.imread("cali_img.jpg")
circles_sorted, crop_points = ivt.run_calibration(cali_img, adjust=False)
cali_circles_init = circles_sorted - circles_sorted[0][0]
cali_circles = []
for circ in cali_circles_init[0]:
cali_circles.append([circ[0], circ[1]])
print "CALI_CIRCLES", cali_circles
p1, inverse = ivt.pix3world_cal(cali_circles[0], cali_circles[2],
cali_circles[1])
CAMERA = 1
ivt.check_camera()
cam_check = raw_input("Change Camera?: ")
if cam_check == "yes":
print "Current camera is " + str(CAMERA)
CAMERA = raw_input("Which Camera to use?: ")
CAMERA = int(CAMERA)
while True:
task = raw_input("task: ")
if task == "wp":
msg = ic.safe_ur_move(c, Pose=dict(i2.pick_k), CMD=2)
#current_Pose = ic.get_ur_position(c,1)
#demand_Pose = {"x":current_Pose[0], "y":current_Pose[1], "z":current_Pose[2], "rx":i1.saucer_waypoint2["rx"], "ry":i1.saucer_waypoint2["ry"], "rz":i1.saucer_waypoint2["rz"]}
#msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
if task == "1":
print "Begin challenge 1..."
i1.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "2":
print "Begin challenge 2..."
i2.begin(c, ser_ee)
if task == "3":
print "Begin challenge 3..."
i3.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "4":
print "Begin challenge 4..."
i4.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "5":
print "Begin challenge 5..."
i5.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "6":
print "Begin challenge 6..."
i6.begin(c, ser_ee)
if task == "7":
print "Begin challenge 7..."
i7.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "8":
print "Begin challenge 8..."
i8.begin(c, ser_ee)
if task == "9":
print "Begin challenge 9..."
i9.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "10":
print "Begin challenge 10..."
i10.begin(c, ser_ee)
if task == "s":
print "Begin challenge s.."
ic.serial_send(ser_ee, "H", 100)
time.sleep(1)
ic.serial_send(ser_ee, "H", 20)
if task == "pose":
current_Pose, current_Grip = ic.get_position(c, ser_ee, 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, CMD=3)
print "current joints: ", current_Joints
print "current grip: ", current_Grip
if task == "grab":
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = int(raw_input("act: "))
demand_Grip["servo"] = int(raw_input("servo: "))
demand_Grip["tilt"] = int(raw_input("tilt: "))
msg = ic.safe_move(c, ser_ee, Grip=demand_Grip, CMD=0)
if task == "calibrate":
while True:
ready = raw_input("Ready?: ")
if ready == "yes":
cali_img = ivt.capture_pic(CAMERA, ROTATION)
circles_sorted, crop_points = ivt.run_calibration(cali_img)
cali_circles_init = circles_sorted - circles_sorted[0][0]
cali_circles = []
for circ in cali_circles_init[0]:
cali_circles.append([circ[0], circ[1]])
print cali_circles
p1, inverse = ivt.pix3world_cal(cali_circles[0],
cali_circles[2],
cali_circles[1])
cv2.imwrite("cali_img.jpg", cali_img)
break
def begin(c, ser_ee, p1, inverse, CAMERA, crop_points):
## Object parameters
cup_radius = 40
cup_height = 60
spoon_bowl = -60 # lenght of spoon bowl (to be convered when stirring)
spoon_height = 11
stir_radius = cup_radius - 20
act_spoon = 75
task_img_3 = ivt.capture_pic(CAMERA, ROTATION)
cv2.imwrite(os.path.join(PATH_TO_TASK_IMAGES, 'task_img_3.jpg'),
task_img_3)
crop_task_img_3 = ivt.crop_out(task_img_3, crop_points)
spoon_mug, spoon_edge_world, empty_cup_centre = ivfunc.find_spoon2(
crop_task_img_3, show=True)
#vision stuff: get mug and saucer position
# mug and saucer centre positions
#mx,my,sx,sy = mug_saucer_pos
## Location of first mug ()
p_pix = [spoon_mug[0], spoon_mug[1]]
print "P_PIX: ", p_pix
px, py = ivt.pix3world(p1, inverse, p_pix)
px = px[0, 0]
py = py[0, 0]
print "SPOON: ", spoon_edge_world
s_pix = [spoon_edge_world[0], spoon_edge_world[1] - 10]
print "S_PIX: ", s_pix
sx, sy = ivt.pix3world(p1, inverse, s_pix)
sx = sx[0, 0]
sy = sy[0, 0]
print "PX, PY, SX, SY: ", px, py, sx, sy
p_centre = [px, py]
p_edge = [sx, sy]
attack_angle = 70
print "P_CENTRE: ", p_centre
print "P_EDGE: ", p_edge
## Location of Second Mug
m_pix = [empty_cup_centre[0], empty_cup_centre[1]]
print "EMPTY_MUG_PIX: ", m_pix
m_pix = [
empty_cup_centre[0],
empty_cup_centre[1] + 0.05 * (250 - empty_cup_centre[1])
]
print "CORRECTED_EMPTY_MUG_PIX: ", m_pix
mx, my = ivt.pix3world(p1, inverse, m_pix)
mx_2 = mx[0, 0]
my_2 = my[0, 0]
print "MX, MY: ", mx, my
# Home for end effector and actuator
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_spoon - 10
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
ic.socket_send(c, sCMD=201)
ic.socket_send(c, sCMD=203)
# Goto spoon (TO FINISH)
x_p, y_p, ori = get_grasping_coords(p_edge, p_centre)
x_p = p_edge[0]
y_p = p_edge[1]
ori = ori + 90
angle_grasp(c, ser_ee, ori, attack_angle)
current_Joints = ic.get_ur_position(c, 3)
if current_Joints[5] > 180:
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] - 90
}
else:
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] + 90
}
msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2)
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": cup_height + spoon_height + 80,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["x"] = x_p
demand_Pose["y"] = y_p
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["z"] = cup_height + spoon_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Grasp spoon
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
demand_Grip["act"] = act_spoon
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(0.5)
# Lift spoon
demand_Pose["z"] = cup_height + spoon_height + 120
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Tilt spoon
msg = ic.safe_ur_move(c, Pose=dict(stir_waypoint_joints), CMD=2)
## Move to second cup x, y
ic.socket_send(c, sCMD=201)
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": mx_2,
"y": my_2,
"z": current_Pose[2],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
## Lower spoon
demand_Pose["z"] = cup_height + spoon_height - spoon_bowl
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
## Stir spoon
add_stir = [0, stir_radius, 0, -stir_radius, 0]
for j in range(0, 3):
for i in range(0, 4):
demand_Pose["x"] = mx_2 + add_stir[i + 1]
demand_Pose["y"] = my_2 + add_stir[i]
msg = ic.safe_ur_move(c, Pose=demand_Pose, Speed=0.15, CMD=4)
## Lift spoon
demand_Pose["z"] = cup_height + spoon_height + 120
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
ic.socket_send(c, sCMD=200)
## Home
msg = ic.safe_move(c, ser_ee, Pose=dict(iw.home_joints), CMD=2)
print ".....................Done......................"
def begin(c, ser_ee, p1, inverse, CAMERA, crop_points):
#vision stuff
task_img_4 = ivt.capture_pic(CAMERA, 3)
cv2.imwrite(os.path.join(PATH_TO_TASK_IMAGES, 'task_img_4.jpg'),
task_img_4)
crop_task_img_4 = ivt.crop_out(task_img_4, crop_points)
CAL_PARAM = {'thresh': [75, 100], 'radius': [30, 45]}
m_circle, m_cimg = ivt.find_circles(copy.copy(crop_task_img_4),
3,
param=CAL_PARAM,
blur=1,
show=False)
plt.imshow(m_cimg)
plt.show()
mx = []
my = []
for mug in range(3):
m_pix = [m_circle[0][mug][0], m_circle[0][mug][1]]
mx_, my_ = ivt.pix3world(p1, inverse, m_pix)
mx.append(mx_[0, 0])
my.append(my_[0, 0])
print "MX: ", mx
print "MY: ", my
#motion stuff: pick mug
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=dict(iw.ee_home),
CMD=2)
# Set tool to iros_1
ic.socket_send(c, sCMD=201)
# Open grabber
ic.serial_send(ser_ee, "H", 100)
# Go to before jug
msg = ic.safe_ur_move(c, Pose=dict(jug_waypoint_joints_1), CMD=2)
time.sleep(0.5)
msg = ic.safe_ur_move(c, Pose=dict(jug_waypoint_joints_2), CMD=2)
# Glose grabber
ic.serial_send(ser_ee, "H", 15)
test = raw_input("wait")
# Lift up
msg = ic.safe_ur_move(c, Pose=dict(jug_waypoint_joints_3), CMD=2)
current_Joints = ic.get_ur_position(c, 3)
for i in range(0, 3):
t1, t2, t3, t4 = get_angles(mx[i], my[i])
print t1, t2, t3, t4
ipt = raw_input("continue?")
demand_Joints = {
"x": t1,
"y": t2,
"z": t3,
"rx": t4,
"ry": current_Joints[4],
"rz": current_Joints[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2)
# pour
demand_Joints["rx"] = t4 + pour_list[i]
msg = ic.safe_ur_move(c, Pose=demand_Joints, Speed=0.1, CMD=2)
# return jug
msg = ic.safe_ur_move(c, Pose=dict(jug_waypoint_joints_3), CMD=2)
msg = ic.safe_ur_move(c, Pose=dict(jug_waypoint_joints_2), CMD=2)
# Open grabber
ic.serial_send(ser_ee, "H", 100)
# move away
msg = ic.safe_ur_move(c, Pose=dict(jug_waypoint_joints_1), CMD=2)
# Set tool to iros_0
ic.socket_send(c, sCMD=200)
# home
msg = ic.safe_move(c, ser_ee, Pose=dict(iw.home_joints), CMD=2)
'''# Go to location of the cup
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_Pose["x"] = mx[0] + pour_offset
demand_Pose["y"] = my[0]
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Pour
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],"rz":current_Joints[5]}
demand_Joints["rx"] = demand_Joints["rx"] + pour_angle_1
msg = ic.safe_ur_move(c,Pose=dict(demand_Joints),CMD=2)
time.sleep(1)
# Stop pour_angle_1
demand_Joints["rx"] = demand_Joints["rx"] - pour_angle_1
msg = ic.safe_ur_move(c,Pose=dict(demand_Joints),CMD=2)
'''
'''
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0], "y":current_Pose[1]+50, "z":current_Pose[2]-50, "rx":current_Pose[3], "ry":current_Pose[4], "rz":current_Pose[5]}
demand_Grip = dict(iw.ee_home)
demand_Grip["servo"]=30
msg = ic.safe_move(c,ser_ee,Pose=demand_Pose,Grip=demand_Grip,CMD=4)
demand_Pose["z"]=current_Pose[2]+50
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
for i in range(0,3):
demand_Pose["x"]=mx[i]+pour_offset
demand_Pose["y"]=my[i]
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
time.sleep(2)
full_jug = measure_av_force(c)
print "average force: ",full_jug
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], "rz":current_Joints[5]}
j=0
while fz < full_jug-2 and j<10:
demand_Joints["rz"]=current_Joints[5]+10*j
msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2,Speed=0.2)
time.sleep(2)
fz = measure_av_force(c)
print "average force: ",fz
demand_Joints["rz"]=current_Joints[5]
msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2)
demand_Pose = {"x":current_Pose[0], "y":current_Pose[1]+50, "z":current_Pose[2]+50, "rx":current_Pose[3], "ry":current_Pose[4], "rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
demand_Pose["z"]=current_Pose[2]-50
demand_Grip["servo"]=120
msg = ic.safe_move(c,ser_ee,Pose=demand_Pose,Grip=demand_Grip,CMD=4)
demand_Pose["y"]=current_Pose[1]
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
msg = ic.safe_ur_move(c,Pose=dict(jug_waypoint_joints),CMD=2)
ic.socket_send(c,sCMD=200)
msg = ic.safe_ur_move(c,Pose=dict(iw.home_joints),CMD=2)
'''
print ".....................Done......................"
def begin(c, ser_ee, p1, inverse, CAMERA, crop_points):
while True:
vision_check = raw_input("Vision? (yes/no)")
if vision_check == "no":
break
elif vision_check == "yes":
task_img_5 = ivt.capture_pic(CAMERA, ROTATION)
cv2.imwrite(os.path.join(PATH_TO_TASK_IMAGES, 'task_img_5.jpg'),
task_img_5)
answer = ivfunc.find_usb(
task_img_5,
crop_points,
params=[USB_REL_CROP, USB_POINTS, USB_SPACE, USB_THRESH])
print "ANSWER: ", answer
answer2 = ivfunc.find_usb(
task_img_5,
crop_points,
params=[PLUG_REL_CROP, PLUG_POINTS, PLUG_SPACE, PLUG_THRESH])
print "ANSWER2: ", answer2
obj = raw_input('Light or USB? (l/u)?')
if (obj == "u"):
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_usb
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
# Set tool to iros_1
ic.socket_send(c, sCMD=201)
# Close Gripper
msg = ic.end_effector_move(ser_ee, demand_Grip)
inp = raw_input("give USB")
if (inp == "1"):
msg = ic.safe_ur_move(c, Pose=dict(usb1_down), CMD=2, Speed=0.2)
if (inp == "2"):
msg = ic.safe_ur_move(c, Pose=dict(usb2_down), CMD=2, Speed=0.2)
if (inp == "3"):
msg = ic.safe_ur_move(c, Pose=dict(usb3_down), CMD=2, Speed=0.2)
if (inp == "4"):
msg = ic.safe_ur_move(c, Pose=dict(usb4_down), CMD=2, Speed=0.2)
# Close Gripper
demand_Grip["servo"] = 18
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(0.5)
raw = raw_input("wait")
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_Pose["z"] = demand_Pose["z"] + 45
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
Speed=0.2,
CMD=4)
time.sleep(2)
# Open Gripper
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(0.5)
'''else:
# Home position
demand_Grip = dict(iw.ee_home)
demand_Grip["act"]=act_light
msg = ic.safe_move(c,ser_ee,Pose=dict(iw.home_joints),Grip=demand_Grip,CMD=2)
# Set tool to iros_1
ic.socket_send(c,sCMD=201)
# Goto position of light
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":pos_light[0], "y":pos_light[1], "z":current_Pose[2], "rx":current_Pose[3], "ry":current_Pose[4], "rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Lower to light
demand_Pose["z"] = height_light
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Close Gripper
demand_Grip["servo"]=30
msg = ic.end_effector_move(ser_ee,demand_Grip)
time.sleep(0.5)
# Pull out
demand_Pose["z"] = height_light + 50
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Push in and release
demand_Pose["z"] = height_light - 5
demand_Grip["servo"]=120
msg = ic.safe_move(c,ser_ee,Pose=demand_Pose,Speed=0.25,Grip=demand_Grip,CMD=4)'''
if (obj == "l"):
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = 47
msg = ic.safe_move(c,
ser_ee,
Pose=dict(mains_waypoint_joints),
Grip=demand_Grip,
CMD=2)
plug = int(raw_input("plug: "))
# Goto position of light
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1] - 28.7 * (plug - 1),
"z": current_Pose[2] - 45,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["x"] = current_Pose[0] - 25
demand_Grip["servo"] = 37
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
time.sleep(0.5)
demand_Pose["z"] = current_Pose[2] + 20
msg = ic.safe_ur_move(c, Pose=demand_Pose, Speed=0.05, CMD=4)
time.sleep(2)
demand_Pose["z"] = current_Pose[2] - 45
demand_Grip["servo"] = 120
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
Speed=0.05,
CMD=4)
time.sleep(0.5)
# Raise
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2] + 80,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Set tool to iros_0
ic.socket_send(c, sCMD=200)
# Home position
msg = ic.safe_move(c, ser_ee, Pose=dict(iw.home_joints), CMD=2)
print ".....................Done......................"
def begin(c, ser_ee, p1, inverse, CAMERA, crop_points):
#object grasping parameters
act_mug = 76
act_saucer = 83
height_mug = 20.0
height_saucer = 5.0
radius_mug = 30.0
radius_saucer = 52.0
zoff = -23.5
task_img_1 = ivt.capture_pic(CAMERA, 3)
cv2.imwrite(os.path.join(PATH_TO_TASK_IMAGES, 'task_img_1.jpg'),
task_img_1)
crop_task_img_1 = ivt.crop_out(task_img_1, crop_points)
table_circles = ivfunc.cup_saucer2(crop_task_img_1, show=True)
print table_circles
print "CROP_POINTS: ", crop_points
print "P1: ", p1
print "INVERSE", inverse
m_circle = table_circles["mug"]["circle"]
s_circle = table_circles["saucer"]["circle"]
print "m_circle: ", m_circle
print "s_circle: ", s_circle
mp = [m_circle[0], m_circle[1]]
mx, my = ivt.pix3world(p1, inverse, mp)
mx = mx[0, 0]
my = my[0, 0]
sp = [s_circle[0], s_circle[1]]
sx, sy = ivt.pix3world(p1, inverse, sp)
sx = sx[0, 0]
sy = sy[0, 0]
#vision stuff: get mug and saucer position
# mug and saucer centre positions
#mx,my,sx,sy = mug_saucer_pos
print "MX: ", mx
print "MY: ", my
# Home
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = 60
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
# Set tool to iros_1
ic.socket_send(c, sCMD=201)
demand_Pose = dict(iw.home)
demand_Pose["x"] = mx + radius_mug / 1.41421
demand_Pose["y"] = my - radius_mug / 1.41421
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["z"] = height_mug + zoff
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Grip["servo"] = 40
msg = ic.end_effector_move(ser_ee, demand_Grip)
demand_Grip["act"] = act_mug
msg = ic.end_effector_move(ser_ee, demand_Grip)
demand_Grip["servo"] = 80
msg = ic.end_effector_move(ser_ee, demand_Grip)
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
demand_Pose["z"] = height_mug + height_saucer + 40 + zoff
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["x"] = sx + radius_mug / 1.41421
demand_Pose["y"] = sy - radius_mug / 1.41421
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["z"] = height_mug + height_saucer + 10 + zoff
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
demand_Pose["z"] = 120 + zoff
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
#motion stuff: pick saucer
demand_Grip["act"] = act_saucer
msg = ic.end_effector_move(ser_ee, demand_Grip)
demand_Joints = dict(saucer_waypoint1_joints)
msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2)
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": sx,
"y": sy + radius_saucer + 30,
"z": current_Pose[2],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["z"] = height_saucer + 40 + zoff
demand_Grip["servo"] = 60
demand_Grip["tilt"] = 28
msg = ic.safe_move(c, ser_ee, Pose=demand_Pose, Grip=demand_Grip, CMD=4)
time.sleep(0.2)
demand_Pose["y"] = sy + radius_saucer
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["z"] = height_saucer + zoff
demand_Grip["servo"] = 30
msg = ic.safe_move(c, ser_ee, Pose=demand_Pose, Grip=demand_Grip, CMD=4)
time.sleep(0.5)
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2],
"rx": saucer_waypoint2["rx"],
"ry": saucer_waypoint2["ry"],
"rz": saucer_waypoint2["rz"]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["z"] = height_saucer + 50 + zoff
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
x = -400 #float(raw_input("x: "))
y = -400 #float(raw_input("y: "))
demand_Pose["x"] = x
demand_Pose["y"] = y + radius_saucer
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["z"] = height_saucer + zoff
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Pose["z"] = height_saucer + zoff
demand_Grip["servo"] = 60
msg = ic.safe_move(c, ser_ee, Pose=demand_Pose, Grip=demand_Grip, CMD=4)
demand_Pose["z"] = height_saucer + 40 + zoff
demand_Grip["servo"] = 120
msg = ic.safe_move(c, ser_ee, Pose=demand_Pose, Grip=demand_Grip, CMD=4)
demand_Pose["z"] = current_Pose[2] + 80
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
ic.socket_send(c, sCMD=200)
msg = ic.safe_move(c, ser_ee, Pose=dict(iw.home_joints), CMD=2)
def begin(c, ser_ee, p1, inverse, CAMERA, crop_points):
msg = ic.safe_move(c, ser_ee, Pose=dict(iw.home_joints), CMD=2)
ic.socket_send(c, sCMD=207)
for i in range(0, 6):
# Vision - Extract the list of shapes
task_img_7 = ivt.capture_pic(CAMERA, 3)
cv2.imwrite(
os.path.join(PATH_TO_TASK_IMAGES, 'task_img_7' + str(i) + '.jpg'),
task_img_7)
crop_task_img_7 = ivt.crop_out(task_img_7, crop_points)
shape_list = ivfunc.extract_shape_list(crop_task_img_7,
threshold=120,
show=True)
if len(shape_list) == 0:
print "No more shapes left"
print "Expected " + str(5 - i) + " more shapes"
continue
piece = shape_list[0]
print "SHAPE INFO: "
print "SHAPE ID: ", piece['shape']
px1 = piece['point1'][0][0]
py1 = piece['point1'][0][1]
px2 = piece['point2'][0][0]
py2 = piece['point2'][0][1]
pix = [px1, py1]
x1, y1 = ivt.pix3world(p1, inverse, pix)
x1 = x1[0, 0]
y1 = y1[0, 0]
pix2 = [px2, py2]
x2, y2 = ivt.pix3world(p1, inverse, pix2)
x2 = x2[0, 0]
y2 = y2[0, 0]
print "X1, Y1, X2, Y2: ", x1, y1, x2, y2
params = [piece['shape'], x1, y1, x2, y2]
x_n, y_n, orient = get_grasping_coords([x1, y1], [x2, y2])
print "X_N, Y_N, ORIENT: "
print x_n, y_n, orient
# Get paramters and put into the following data structure
# Get paramters and put into the following data structure
#params = [0, x_pos, y_pos, ori] # where the first item is the number that states the object type: 0 = cirlce, 1 = rect, 2 = triangle etc.
## Set orientation
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],
"rz": current_Joints[5] - orient + 135
}
msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2)
# Go to X,Y centre of the location
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": x1,
"y": y1,
"z": current_Pose[2],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_objects[params[0]] - 5
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
# Move down to Grasp
current_Pose = ic.get_ur_position(c, 1)
demand_Pose["z"] = height_pick
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
## Partially close Gripper
demand_Grip["servo"] = 40
msg = ic.end_effector_move(ser_ee, demand_Grip)
## Close Actuator
demand_Grip["act"] = act_objects[params[0]]
msg = ic.end_effector_move(ser_ee, demand_Grip)
## Close Gripper
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(0.5)
# Pick up object
demand_Pose["z"] = height_pick + 90
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
if (params[0] == 0):
# Move to peg holder
location = peg_loc[params[0]]
#demand_Pose["x"] = location[0]
#demand_Pose["y"] = location[1]
msg = ic.safe_ur_move(c, Pose=dict(circle_way), CMD=2, Speed=0.4)
if (params[0] == 1):
# Move to peg holder
location = peg_loc[params[0]]
#demand_Pose["x"] = location[0]
#demand_Pose["y"] = location[1]
msg = ic.safe_ur_move(c, Pose=dict(rect_way), CMD=2, Speed=0.4)
if (params[0] == 2):
# Move to peg holder
location = peg_loc[params[0]]
#demand_Pose["x"] = location[0]
#demand_Pose["y"] = location[1]
msg = ic.safe_ur_move(c, Pose=dict(tri_way), CMD=2, Speed=0.1)
if (params[0] == 3):
# Move to peg holder
location = peg_loc[params[0]]
#demand_Pose["x"] = location[0]
#demand_Pose["y"] = location[1]
msg = ic.safe_ur_move(c, Pose=dict(square_way), CMD=2, Speed=0.4)
if (params[0] == 4):
# Move to peg holder
location = peg_loc[params[0]]
#demand_Pose["x"] = location[0]
#demand_Pose["y"] = location[1]
msg = ic.safe_ur_move(c, Pose=dict(pent_way), CMD=2, Speed=0.4)
'''
# Rotate to zero wrist orientation
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],"rz":0}
msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2)
# Home rotation
current_Pose = ic.get_ur_position(c,1)
demand_Pose = dict(iw.home)
demand_Pose["x"]=current_Pose[0]
demand_Pose["y"]=current_Pose[1]
demand_Pose["z"]=current_Pose[2]
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Rotate to zero orientation
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],"rz":current_Joints[5]-45}
msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2)
#test = raw_input("sasdf")
'''
# Lower
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": height_place,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
Speed=0.2,
CMD=4)
# Release
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
# Raise
demand_Pose["z"] = height_place + 70
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
CMD=4)
msg = ic.safe_ur_move(c, Pose=dict(iw.home_joints), CMD=2)
ic.socket_send(c, sCMD=200)
# Raise from peg
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
print ".....................Done......................"