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 main():
c, ser_ee = initialize()
# loop
print c.recv(1024)
inp = raw_input("Continue?")
msg = ic.safe_move(c, ser_ee, Pose=dict(iw.home_joints), CMD=2)
cali_img = cv2.imread("cali_img.jpg")
circles_sorted, crop_points = ivt.run_calibration(cali_img, adjust=False)
cali_circles_init = circles_sorted - circles_sorted[0][0]
cali_circles = []
for circ in cali_circles_init[0]:
cali_circles.append([circ[0], circ[1]])
print "CALI_CIRCLES", cali_circles
p1, inverse = ivt.pix3world_cal(cali_circles[0], cali_circles[2],
cali_circles[1])
CAMERA = 1
ivt.check_camera()
cam_check = raw_input("Change Camera?: ")
if cam_check == "yes":
print "Current camera is " + str(CAMERA)
CAMERA = raw_input("Which Camera to use?: ")
CAMERA = int(CAMERA)
while True:
task = raw_input("task: ")
if task == "wp":
msg = ic.safe_ur_move(c, Pose=dict(i2.pick_k), CMD=2)
#current_Pose = ic.get_ur_position(c,1)
#demand_Pose = {"x":current_Pose[0], "y":current_Pose[1], "z":current_Pose[2], "rx":i1.saucer_waypoint2["rx"], "ry":i1.saucer_waypoint2["ry"], "rz":i1.saucer_waypoint2["rz"]}
#msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
if task == "1":
print "Begin challenge 1..."
i1.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "2":
print "Begin challenge 2..."
i2.begin(c, ser_ee)
if task == "3":
print "Begin challenge 3..."
i3.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "4":
print "Begin challenge 4..."
i4.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "5":
print "Begin challenge 5..."
i5.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "6":
print "Begin challenge 6..."
i6.begin(c, ser_ee)
if task == "7":
print "Begin challenge 7..."
i7.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "8":
print "Begin challenge 8..."
i8.begin(c, ser_ee)
if task == "9":
print "Begin challenge 9..."
i9.begin(c, ser_ee, p1, inverse, CAMERA, crop_points)
if task == "10":
print "Begin challenge 10..."
i10.begin(c, ser_ee)
if task == "s":
print "Begin challenge s.."
ic.serial_send(ser_ee, "H", 100)
time.sleep(1)
ic.serial_send(ser_ee, "H", 20)
if task == "pose":
current_Pose, current_Grip = ic.get_position(c, ser_ee, CMD=1)
print "current pose: ", current_Pose
print "current grip: ", current_Grip
if task == "joints":
current_Joints, current_Grip = ic.get_position(c, ser_ee, CMD=3)
print "current joints: ", current_Joints
print "current grip: ", current_Grip
if task == "grab":
demand_Grip = dict(iw.ee_home)
demand_Grip["act"] = int(raw_input("act: "))
demand_Grip["servo"] = int(raw_input("servo: "))
demand_Grip["tilt"] = int(raw_input("tilt: "))
msg = ic.safe_move(c, ser_ee, Grip=demand_Grip, CMD=0)
if task == "calibrate":
while True:
ready = raw_input("Ready?: ")
if ready == "yes":
cali_img = ivt.capture_pic(CAMERA, ROTATION)
circles_sorted, crop_points = ivt.run_calibration(cali_img)
cali_circles_init = circles_sorted - circles_sorted[0][0]
cali_circles = []
for circ in cali_circles_init[0]:
cali_circles.append([circ[0], circ[1]])
print cali_circles
p1, inverse = ivt.pix3world_cal(cali_circles[0],
cali_circles[2],
cali_circles[1])
cv2.imwrite("cali_img.jpg", cali_img)
break
def begin(c, ser_ee, p1, inverse, CAMERA, crop_points):
#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......................"