def select_location(c, thok, angle):
thok.servo.move_angle(angle)
cvim = c.get_frame()
cvim = c.get_frame()
cvim = c.get_frame()
im_angle = thok.servo.read_angle()
tilt_angles = (math.radians(-20) + angle, math.radians(30) + angle)
pos_list, scan_list = thok.scan(tilt_angles, speed=math.radians(10))
m = p3d.generate_pointcloud(pos_list, scan_list, math.radians(-60),
math.radians(60), 0.0, -0.055)
pts = mcf.utmcam0Tglobal(mcf.globalTthok0(m), im_angle)
cam_params = cc.camera_parameters['mekabotUTM']
fx = cam_params['focal_length_x_in_pixels']
fy = cam_params['focal_length_y_in_pixels']
cx, cy = cam_params['optical_center_x_in_pixels'], cam_params[
'optical_center_y_in_pixels']
cam_proj_mat = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
cvim, pts2d = cul.project_points_in_image(cvim, pts, cam_proj_mat)
cp = cul.get_click_location(cvim)
print 'Clicked location:', cp
if cp == None:
return None
idx = cul.get_index(pts2d.T, cp)
pt3d = pts[:, idx]
pt_interest = mcf.globalTutmcam0(pt3d, im_angle)
hl_thok0 = mcf.thok0Tglobal(pt_interest)
l1, l2 = 0.0, -0.055
d = {}
d['pt'] = hl_thok0
d['pos_list'], d['scan_list'] = pos_list, scan_list
d['l1'], d['l2'] = l1, l2
d['img'] = uto.cv2np(cvim)
ut.save_pickle(d, 'hook_plane_scan_' + ut.formatted_time() + '.pkl')
return pt_interest
sys.exit()
hook_location = None
#z_list = [1.0,0.5]
z_list = [opt.z]
i = 0
while hook_location == None:
if i == len(z_list):
print 'Did not get a click. Exiting...'
sys.exit()
z = z_list[i]
cmg.z.torque_move_position(z)
hook_location = lpi.select_location(cmg.cam,cmg.thok,math.radians(sa))
i += 1
hl_thok0 = mcf.thok0Tglobal(hook_location)
hl_torso = mcf.torsoTglobal(hook_location)
t_begin = time.time()
angle = 0.
pull_loc,residual_angle = cmg.reposition_robot(hl_torso,angle,math.radians(ha),
position_number=pnum)
print 'pull_loc:',pull_loc.A1.tolist()
starting_location = copy.copy(hl_torso)
starting_angle = -angle
pose_dict = {}
pose_dict['loc'] = starting_location
pose_dict['angle'] = angle
pose_dict['residual_angle'] = residual_angle
pose_dict['position_number'] = pnum
print 'creating placement object'
pl = Placement(
pc, resolution) ###REPLACE WITH MY OWN CLASS DEFINITION WITH FUNCTIONs
if displayOn:
placement_point = pl.test_placement(polygon, object_height)
else:
placement_point = pl.find_placement(
polygon, object_height) #Add param True to get debug popups
placement_point -= pc.scan_dataset.ground_plane_translation
#Assumes 'codyRobot'==ROBOT
#This should be optional
import mekabot.coord_frames as mcf
placement_point_global = mcf.thok0Tglobal(placement_point)
print 'placement point in global coordinate frame:', placement_point_global.T
if displayOn:
print 'displaying current placement'
pl.display_current_placement_in_heightmap()
cv.highgui.cvWaitKey()
if displayOn:
print 'importing mayavi'
###from enthought.mayavi import mlab
print 'showing 3D mayavi'
###mlab.show()
#What does this do?
print getTime(), 'DONE'
