def begin(c, ser_ee):
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)
msg = ic.safe_ur_move(c, Pose=dict(usb1_down), CMD=2, Speed=0.2)
# Close Gripper
demand_Grip["servo"] = 30
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"] + 30
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Grip=demand_Grip,
Speed=0.2,
CMD=4)
time.sleep(2)
msg = ic.safe_ur_move(c, Pose=dict(usb1_down), CMD=2, Speed=0.2)
raw = raw_input("wati")
# Open Gripper
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(0.5)
#Pull out
demand_Pose["z"] = height_u + 50
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
time.sleep(5)
#Push in and release
demand_Pose["z"] = height_u - 5
demand_Grip["servo"] = 120
msg = ic.safe_move(c,
ser_ee,
Pose=demand_Pose,
Speed=0.25,
Grip=demand_Grip,
CMD=4)
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)
# 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):
x_n, y_n, orient = get_grasping_coords([x1, y1], [x2, y2])
# Home
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_straw
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
ic.socket_send(c, sCMD=201)
# Move to straw location
msg = ic.safe_ur_move(c, Pose=dict(grasp), CMD=2)
# Grasp straw
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(0.5)
# Lift straw
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2] + 250,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Move to drop straw
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": -200,
"y": -400,
"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)
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": -200,
"y": -400,
"z": 150,
"rx": current_Pose[3],
"ry": current_Pose[4] + 50,
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Drop straw
demand_Grip["servo"] = 100
msg = ic.end_effector_move(ser_ee, demand_Grip)
msg = ic.safe_ur_move(c, Pose=dict(iw.home_joints), CMD=2)
## 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_straw
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)
## Close Gripper
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(0.5)
# Lift straw
demand_Pose["z"] = height_pick + 100
msg = ic.safe_move(c, ser_ee, Pose=demand_Pose, Grip=demand_Grip, CMD=4)
# Way point 1
#msg = ic.safe_ur_move(c,Pose=dict(way_1),CMD=2)
# Way point 2
#msg = ic.safe_ur_move(c,Pose=dict(way_2),CMD=2)
# Lower into cup
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": 100,
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
msg = ic.safe_ur_move(c, Pose=demand_Pose, Speed=0.2, CMD=4)
def begin(c, ser_ee):
# pick up fork
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_f
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
# ipt = raw_input("continue")
# Go to just above fork place_height
msg = ic.safe_move(c, ser_ee, Pose=dict(pick_f), CMD=2)
#ipt = raw_input("continue")
#Go down in the z to pick up the fork
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0] + 17,
"y": current_Pose[1],
"z": current_Pose[2],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
demand_Pose["z"] = pick_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Close the gripper
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# ipt = raw_input("continue")
# Raise
msg = ic.safe_ur_move(c, Pose=dict(pick_f), CMD=2)
#ipt = raw_input("continue")
# Go to place the fork
msg = ic.safe_ur_move(c, Pose=dict(place_f), CMD=2)
#ipt = raw_input("continue")
# Lower the fork
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"] = place_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Release
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
#ipt = raw_input("continue")
# Raise arm
msg = ic.safe_ur_move(c, Pose=dict(place_f), CMD=2)
#test = raw_input("Go on to spoon?")
# Return Fork
# Lower
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"] = place_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Pick up fork
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Lift and move above the tray for fokr
msg = ic.safe_ur_move(c, Pose=dict(place_f), CMD=2)
msg = ic.safe_ur_move(c, Pose=dict(pick_f), CMD=2)
# Lower
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"] = pick_height + 50
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Release
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Raise
msg = ic.safe_ur_move(c, Pose=dict(pick_k), CMD=2)
###############################################################################
# pick up spoon
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_s
msg = ic.end_effector_move(ser_ee, demand_Grip)
# Go to just above spoon place_height
msg = ic.safe_ur_move(c, Pose=dict(pick_s), CMD=2)
#Go down in the z to pick up the fork
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0] + 12,
"y": current_Pose[1],
"z": current_Pose[2],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
demand_Pose["z"] = pick_height + 2
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Close the gripper
demand_Grip["servo"] = 25
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Raise
msg = ic.safe_ur_move(c, Pose=dict(pick_s), CMD=2)
# Go to place the fork
msg = ic.safe_ur_move(c, Pose=dict(place_s), CMD=2)
# Lower the fork
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"] = place_height + 5
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Release
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Raise arm
msg = ic.safe_ur_move(c, Pose=dict(place_s), CMD=2)
#test = raw_input("Go on to knife?")
# Go abouve place of spoon
msg = ic.safe_ur_move(c, Pose=dict(place_s), CMD=2)
# Lower
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"] = place_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Pick up fork
demand_Grip["servo"] = 25
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Lift and move above the tray for fokr
msg = ic.safe_ur_move(c, Pose=dict(place_s), CMD=2)
msg = ic.safe_ur_move(c, Pose=dict(pick_s), CMD=2)
# Lower
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"] = pick_height + 20
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Release
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Raise
msg = ic.safe_ur_move(c, Pose=dict(pick_s), CMD=2)
test = raw_input("Go on to retrieve knife?")
###############################################################################
# pick up knife
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_k
msg = ic.end_effector_move(ser_ee, demand_Grip)
# Go to just above spoon place_height
demand_Pose = dict(pick_k)
demand_Pose["x"] = demand_Pose["x"] + 20
msg = ic.safe_ur_move(c, Pose=dict(pick_k), CMD=2)
# Go to low ish and then mostly close gripper
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0] - 2,
"y": current_Pose[1],
"z": current_Pose[2],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
demand_Pose["z"] = pick_height + 15
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
demand_Grip["servo"] = 50
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Move sideways
demand_Pose = ic.get_ur_position(c, 1)
demand_Pose["x"] = demand_Pose["x"]
msg = ic.safe_ur_move(c, Pose=dict(pick_k), CMD=2)
#Go down in the z to pick up the fork
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0] - 2,
"y": current_Pose[1],
"z": current_Pose[2],
"rx": current_Pose[3],
"ry": current_Pose[4],
"rz": current_Pose[5]
}
demand_Pose["z"] = pick_height - 2
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Close the gripper
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Raise
msg = ic.safe_ur_move(c, Pose=dict(pick_k), CMD=2)
# Go to place the fork
msg = ic.safe_ur_move(c, Pose=dict(place_k), CMD=2)
# Lower the fork
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"] = place_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Release
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Raise arm
msg = ic.safe_ur_move(c, Pose=dict(place_k), CMD=2)
test = raw_input("Go on to retrieve fork?")
###############################################################################
# pick up fork
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_f
msg = ic.end_effector_move(ser_ee, demand_Grip)
test = raw_input("Go on to retrieve spoon?")
###############################################################################
# pick up spoon
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_s
msg = ic.end_effector_move(ser_ee, demand_Grip)
###############################################################################
# pick up knife
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_k
msg = ic.end_effector_move(ser_ee, demand_Grip)
# Go abouve place of knife
msg = ic.safe_ur_move(c, Pose=dict(place_k), CMD=2)
# Lower
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"] = place_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Pick up fork
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Lift and move above the tray for fokr
msg = ic.safe_ur_move(c, Pose=dict(place_k), CMD=2)
msg = ic.safe_ur_move(c, Pose=dict(pick_k), CMD=2)
# Lower
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"] = pick_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Release
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(1)
# Raise
msg = ic.safe_ur_move(c, Pose=dict(pick_k), CMD=2)
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 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):
# Go To home position
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_paper
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
# Go to kitchen paper location
ori = [0, 45, 90, 135, 180]
dx = r * math.cos(ori) * 180 / math.pi
dy = r * math.sin(ori) * 180 / math.pi
move_dx = r * math.cos(ori) * 180 / math.pi
move_dy = r * math.sin(ori) * 180 / math.pi
for i in range(0, len(ori) + 1):
# Home
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
# Set rotations
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] - ori + 135
}
msg = ic.safe_ur_move(c, Pose=demand_Joints, CMD=2)
# Move to abouve the point
current_Pose = ic.get_ur_position(c, 1)
demand_Pose["x"] = centre[0] + dx[i]
demand_Pose["y"] = centre[1] + dy[i]
demand_Pose["z"] = height + 50
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=4)
# Move down
demand_Pose["z"] = height
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=4)
# Grip
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
time.sleep(0.5)
# Tear
demand_Pose["x"] = centre[0] + move_dx[i]
demand_Pose["y"] = centre[1] + move_dy[i]
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=4)
# Release
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
# Move up
demand_Pose["z"] = height + 50
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=4)
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):
# Home
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = act_straw
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
ic.socket_send(c, sCMD=201)
# Go to waypoint to get correct orientation
demand_Pose = dict(iw.grabbing_joints_waypoint)
demand_Pose["x"] = cup_1[0]
msg = ic.safe_move(c, ser_ee, Pose=demand, Grip=demand_Grip, CMD=2)
# Keep same joint orientation and just move in x, y and z
#Move to straw height_mug
demand_Pose["z"] = straw_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Move to straw location
demand_Pose["y"] = cup_1[1] + cup1_offset
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
#Grasp straw
demand_Grip["servo"] = 30
msg = ic.end_effector_move(ser_ee, demand_Grip)
# Lift Straw
demand_Pose["z"] = straw_height + 200
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Move Straw
demand_Pose["x"] = cup_2[0]
demand_Pose["y"] = cup_2[1]
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
# Lower straw into cup
demand_Pose["z"] = cup2_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Release straw
demand_Grip["servo"] = 120
msg = ic.end_effector_move(ser_ee, demand_Grip)
# Move up
demand_Pose["z"] = cup2_height
msg = ic.safe_ur_move(c, Pose=demand_Pose, CMD=4)
# Return home
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
def begin(c,ser_ee):
# parameters
bottle_radius = 25
bottle_height = 70
act_bottle = 35
# Parameters to pass to the function
bx_1 = -80
by_1 = -590
demand_Grip = dict(iw.ee_home)
demand_Grip["act"]=act_bottle
msg = ic.safe_move(c,ser_ee,Pose=dict(iw.home_joints),Grip=demand_Grip,CMD=2)
# Set tool to iros_10
ic.socket_send(c,sCMD=210)
# Goto XY position for the bottle
demand_Pose = dict(iw.home)
demand_Pose["x"] = bx_1
demand_Pose["y"] = by_1
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
#stored_Joints = ic.get_ur_position(c,3)
# Twist lid
#tcp_rotate(c)
for i in range(0,2):
for j in range(0,5):
msg = ic.safe_ur_move(c,Pose=dict(lid_joints[j]),Speed=0.15,CMD=2)
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":current_Pose[2]+30,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Raise
'''current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":current_Pose[2]+30,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Repeat
# Goto XY position for the bottle
demand_Joints = {"x":stored_Joints[0],"y":stored_Joints[1],"z":stored_Joints[2],"rx":stored_Joints[3],"ry":stored_Joints[4],"rz":stored_Joints[5]}
msg = ic.safe_ur_move(c,Pose=demand_Joints,CMD=2)
# Twist lid
tcp_rotate(c)'''
# Set tool to iros_1
ic.socket_send(c,sCMD=201)
# Raise
msg = ic.safe_ur_move(c,Pose=dict(iw.home_joints),CMD=2)
demand_Pose = dict(iw.home)
demand_Pose["x"] = bx_1 + bottle_radius/1.414
demand_Pose["y"] = by_1 - bottle_radius/1.414
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Grasp lid
demand_Pose["z"]=bottle_height
demand_Grip["servo"]=30
msg = ic.safe_move(c,ser_ee,Pose=dict(demand_Pose),Grip=demand_Grip,CMD=4)
time.sleep(0.5)
# Lift up
demand_Pose = dict(iw.home)
demand_Pose["x"] = bx_1 + bottle_radius/1.414
demand_Pose["y"] = by_1 - bottle_radius/1.414
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),CMD=4)
# Set tool to iros_0
ic.socket_send(c,sCMD=200)
# Home
msg = ic.safe_ur_move(c,Pose=dict(iw.home_joints),CMD=2)
# Release bottle lid
demand_Grip["servo"]=120
msg = ic.end_effector_move(ser_ee,demand_Grip)
# 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):
#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......................"