def begin(c, ser_ee):
# Home
msg = ic.safe_ur_move(c, Pose=dict(iw.home_joints), CMD=2)
ic.serial_send(ser_ee, "H", 100)
# Move towards hammer using waypoints
msg = ic.safe_ur_move(c, Pose=dict(hammer_waypoint_joints_1), CMD=2)
msg = ic.safe_ur_move(c, Pose=dict(hammer_waypoint_joints_2), CMD=2)
time.sleep(1)
raw = raw_input("continue?")
msg = ic.safe_ur_move(c,
Pose=dict(hammer_waypoint_joints_3),
CMD=2,
Speed=0.1)
# Close hammer servo
ic.serial_send(ser_ee, "H", 10)
time.sleep(2)
raw = raw_input("continue?")
msg = ic.safe_ur_move(c, Pose=dict(nail_1), CMD=2, Speed=0.2)
updown(c)
msg = ic.safe_ur_move(c, Pose=dict(nail_2), CMD=2, Speed=0.2)
updown(c)
msg = ic.safe_ur_move(c, Pose=dict(nail_3), CMD=2, Speed=0.2)
updown(c)
msg = ic.safe_ur_move(c, Pose=dict(nail_4), CMD=2, Speed=0.2)
updown(c)
msg = ic.safe_ur_move(c, Pose=dict(nail_5), CMD=2, Speed=0.2)
updown(c)
msg = ic.safe_ur_move(c, Pose=dict(nail_6), CMD=2, Speed=0.2)
updown(c)
# Release hammer
ic.serial_send(ser_ee, "H", 100)
# GO HOme
msg = ic.safe_move(c,
ser_ee,
Pose=dict(iw.home_joints),
Grip=demand_Grip,
CMD=2)
ic.socket_send(c, sCMD=200)
msg = ic.safe_move(c, ser_ee, Pose=dict(iw.home_joints), CMD=2)
print ".....................Done......................"
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, 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):
## 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):
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......................"
