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'