def measure_av_force(c, tolerance=0.01):
n = 0
fz = 0.0
while True:
force = ic.get_ur_position(c, 6)
if abs(fz / float(n) - force[2]) < tolerance:
break
fz = fz + force[2]
n = n + 1
print "fz: ", fz / float(n)
print "n: ", n
return fz / float(n)
def updown(c):
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"] - nail_down
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=4, Speed=0.2)
time.sleep(1)
demand_Pose["z"] = demand_Pose["z"] + nail_down
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=4, Speed=0.2)
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):
# 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):
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 _move_grabber_to_angle(self, angle): # assume pointing straight down
joint_pos = ic.get_ur_position(self.conn, 3)
joint_pos[5] -= angle # rotate end joint by angle
ic.safe_ur_move(self.conn, Pose=joint_pos, CMD=2)
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):
## 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 angle_grasp(c, ser_ee, orientation, angle_of_attack):
# Break-up rotations into max 90degrees
thetaz = 0
if orientation > 90:
orientation = orientation - 90
thetaz = math.pi / 2
elif orientation < -90:
orientation = orientation + 90
thetaz = -math.pi / 2
# Avoid singularity at +/-45degrees
if orientation == 45:
orientation = 44
elif orientation == -45:
orientation = -44
# Convert to radians
angle_of_attack = angle_of_attack * math.pi / 180.0
orientation = orientation * math.pi / 180.0
thetay = 135.0 * math.pi / 180.0
# Cartesian rotation matrices to match uw.grabbing_joints rotation
x_rot = np.matrix([[1.0, 0.0, 0.0],
[0.0,
math.cos(math.pi / 2), -math.sin(math.pi / 2)],
[0.0, math.sin(math.pi / 2),
math.cos(math.pi / 2)]]) # x_rot[rows][columns]
y_rot = np.matrix([[math.cos(thetay), 0.0, -math.sin(thetay)],
[0.0, 1.0, 0.0],
[math.sin(thetay), 0.0,
math.cos(thetay)]]) # y_rot[rows][columns]
z_rot = np.matrix([[math.cos(0.0), -math.sin(0.0), 0.0],
[math.sin(0.0), math.cos(0.0), 0.0],
[0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Move to grabbing waypoint
msg = ic.safe_ur_move(c,
Pose=dict(iw.grabbing_joints_waypoint),
Speed=1.0,
CMD=2)
# Create rotation matrix for current position
R = z_rot * y_rot * x_rot
if thetaz != 0:
# Axis rotation matricies for grasping position, rotate around x-axis by aoa, then z-axis by ori
x_rot = np.matrix(
[[1.0, 0.0, 0.0],
[0.0, math.cos(angle_of_attack), -math.sin(angle_of_attack)],
[0.0, math.sin(angle_of_attack),
math.cos(angle_of_attack)]]) # x_rot[rows][columns]
z_rot = np.matrix([[math.cos(thetaz), -math.sin(thetaz), 0.0],
[math.sin(thetaz),
math.cos(thetaz), 0.0],
[0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Cartesian rotation matrix of desired orientation
R = z_rot * x_rot * R
# Cartesian to axis-angle
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1.0) / 2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta * 180 / math.pi
ry = multi * (R[0, 2] - R[2, 0]) * theta * 180 / math.pi
rz = multi * (R[1, 0] - R[0, 1]) * theta * 180 / math.pi
print rx, ry, rz
# Rotate around tool centre point defined by tcp_2
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2],
"rx": rx,
"ry": ry,
"rz": rz
}
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=8)
# Axis rotation matricies for grasping position, rotate around x-axis by aoa, then z-axis by ori
z_rot = np.matrix(
[[math.cos(orientation), -math.sin(orientation), 0.0],
[math.sin(orientation),
math.cos(orientation), 0.0], [0.0, 0.0,
1.0]]) # z_rot[rows][columns]
# Cartesian rotation matrix of desired orientation
R = z_rot * R
# Cartesian to axis-angle
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1.0) / 2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta * 180 / math.pi
ry = multi * (R[0, 2] - R[2, 0]) * theta * 180 / math.pi
rz = multi * (R[1, 0] - R[0, 1]) * theta * 180 / math.pi
print rx, ry, rz
# Rotate around tool centre point defined by tcp_2
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2],
"rx": rx,
"ry": ry,
"rz": rz
}
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=8)
else:
# Axis rotation matricies for grasping position, rotate around x-axis by aoa, then z-axis by ori
x_rot = np.matrix(
[[1.0, 0.0, 0.0],
[0.0, math.cos(angle_of_attack), -math.sin(angle_of_attack)],
[0.0, math.sin(angle_of_attack),
math.cos(angle_of_attack)]]) # x_rot[rows][columns]
z_rot = np.matrix(
[[math.cos(orientation), -math.sin(orientation), 0.0],
[math.sin(orientation),
math.cos(orientation), 0.0], [0.0, 0.0,
1.0]]) # z_rot[rows][columns]
# Cartesian rotation matrix of desired orientation
R = z_rot * x_rot * R
# Cartesian to axis-angle
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1.0) / 2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta * 180 / math.pi
ry = multi * (R[0, 2] - R[2, 0]) * theta * 180 / math.pi
rz = multi * (R[1, 0] - R[0, 1]) * theta * 180 / math.pi
print rx, ry, rz
# Rotate around tool centre point defined by tcp_2
current_Pose = ic.get_ur_position(c, 1)
demand_Pose = {
"x": current_Pose[0],
"y": current_Pose[1],
"z": current_Pose[2],
"rx": rx,
"ry": ry,
"rz": rz
}
msg = ic.safe_ur_move(c, Pose=dict(demand_Pose), CMD=8)
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):
# 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 tcp_rotate(c):
# Cartesian rotation matrices to match grabbing_joints rotation
x_rot = np.matrix([[ 1.0, 0.0, 0.0],
[ 0.0, math.cos(math.pi-0.001), -math.sin(math.pi-0.001)],
[ 0.0, math.sin(math.pi-0.001), math.cos(math.pi-0.001)]]) # x_rot[rows][columns]
y_rot = np.matrix([[ math.cos(0.0), 0.0, -math.sin(0.0)],
[ 0.0, 1.0, 0.0],
[ math.sin(0.0), 0.0, math.cos(0.0)]]) # y_rot[rows][columns]
z_rot = np.matrix([[ math.cos(-7*math.pi/4), -math.sin(-7*math.pi/4), 0.0],
[ math.sin(-7*math.pi/4), math.cos(-7*math.pi/4), 0.0],
[ 0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Create rotation matrix for current position
R=z_rot*y_rot*x_rot
# Cartesian to axis-angle
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1.0)/2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta * 180/math.pi
ry = multi * (R[0, 2] - R[2, 0]) * theta * 180/math.pi
rz = multi * (R[1, 0] - R[0, 1]) * theta * 180/math.pi
#print rx, ry, rz
#inp = raw_input("Continue?")
# Move to drawing waypoint
#msg = ic.safe_ur_move(c,Pose=dict(uw.naughts_crosses_cam_joints),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":rx,"ry":ry,"rz":rz}
msg = ic.safe_ur_move(c,Pose=demand_Pose,Speed=1.5,CMD=8)
# Axis rotation matricies for grasping position, rotate around x-axis by aoa, then z-axis by ori
#x_rot = np.matrix([[ 1.0, 0.0, 0.0],
# [ 0.0, math.cos(angle_of_attack), -math.sin(angle_of_attack)],
# [ 0.0, math.sin(angle_of_attack), math.cos(angle_of_attack)]]) # x_rot[rows][columns]
# Move down until oject is reached
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":40,"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
print "sending force_move................................................"
msg = ic.safe_ur_move(c,Pose=dict(demand_Pose),Speed=0.05,CMD=5)
time.sleep(0.5)
current_Pose = ic.get_ur_position(c,1)
for i in range(0,4):
z_rot = np.matrix([[ math.cos(math.pi/2), -math.sin(math.pi/2), 0.0],
[ math.sin(math.pi/2), math.cos(math.pi/2), 0.0],
[ 0.0, 0.0, 1.0]]) # z_rot[rows][columns]
# Cartesian rotation matrix of desired orientation
R=z_rot*R
# Cartesian to axis-angle
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1.0)/2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta * 180/math.pi
ry = multi * (R[0, 2] - R[2, 0]) * theta * 180/math.pi
rz = multi * (R[1, 0] - R[0, 1]) * theta * 180/math.pi
#print rx, ry, rz
#inp = raw_input("Continue?")
# Rotate around tool centre point defined by tcp_2
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":current_Pose[2],"rx":rx,"ry":ry,"rz":rz}
msg = ic.safe_ur_move(c,Pose=demand_Pose,Speed=1.5,CMD=8)
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......................"
