def get_tfm(self, master_node, child_node, angle): # angle in degrees mtfm = self.masterG.edge[master_node]["cor"]['tfm'] angle = utils.rad_angle(angle) rot = np.eye(4) rot[0:3,0:3] = utils.rotation_matrix(np.array([0,0,1]), angle*self.childN["angle_scale"]) ctfm = self.childN["graph"].edge[self.childN["primary"]][child_node]['tfm'] return mtfm.dot(rot).dot(self.gpTfm).dot(ctfm)
def get_tfm(master_node, child_node, angle): # angle in degrees mprimary_node = mG_opt.node[master]["primary"] #print masterG.edge[master_node][mprimary_node] mtfm = masterG.edge[master_node][mprimary_node]['tfm'] angle = utils.rad_angle(angle) rot = np.eye(4) rot[0:3,0:3] = utils.rotation_matrix(np.array([0,0,1]), angle*mG_opt.node[group]["angle_scale"]) cprimary_node = mG_opt.node[group]["primary"] ctfm = mG_opt.node[group]["graph"].edge[cprimary_node][child_node]['tfm'] return mtfm.dot(rot).dot(mG_opt.edge[master][group]['tfm']).dot(ctfm)
def f_objective (X): """ Objective function to make transforms close to average transforms. Sum of the norms of matrix differences between each Tij and """ obj = 0 zaxis = np.array([0,0,1]) for g1,g2 in mG.edges_iter(): for angle in mG.edge[g1][g2]['avg_tfm']: t1 = np.r_[get_mat_from_node(X,g1),np.array([[0,0,0,1]])] t2 = np.r_[get_mat_from_node(X,g2),np.array([[0,0,0,1]])] rad = utils.rad_angle(angle) tot_angle = rad*(-mG.node[g1]["angle_scale"]+mG.node[g2]["angle_scale"]) rot = np.eye(4) rot[0:3,0:3] = utils.rotation_matrix(zaxis, tot_angle) tfm = t1.dot(rot).dot(nlg.inv(t2)) obj += nlg.norm(tfm - mG.edge[g1][g2]['avg_tfm'][angle]) return obj
def test_gc2 (n, add_noise=False): ## # Markers 1,2,3 are on l finger # Markers 4,5,6 are on r finger # 7 and 8 are on the middle ## ### Initializing everything # create calib info calib_info = {"master": {"ar_markers":[7,8], "master_group":True, "angle_scale":0}, "l": {"ar_markers":[1,2,3], "angle_scale":1}, "r": {"ar_markers":[4,5,6], "angle_scale":-1}} masterGraph = nx.DiGraph() # Setup the groups for group in calib_info: masterGraph.add_node(group) masterGraph.node[group]["graph"] = nx.Graph() masterGraph.node[group]["angle_scale"] = calib_info[group]['angle_scale'] if calib_info[group].get("ar_markers") is None: calib_info[group]["ar_markers"] = [] if calib_info[group].get("hydras") is None: calib_info[group]["hydras"] = [] if calib_info[group].get("ps_markers") is None: calib_info[group]["ps_markers"] = [] if calib_info[group].get("master_group") is not None: masterGraph.node[group]["master"] = 1 masterGraph.node[group]['angle_scale'] = 0 masterGraph.node[group]["markers"] = calib_info[group]["ar_markers"] + calib_info[group]["hydras"] + calib_info[group]["ps_markers"] masterGraph.node[group]["calib_info"] = calib_info[group] ### Done initializing. T12 = make_obs() T23 = make_obs() T45 = make_obs() T56 = make_obs() T78 = make_obs() T7cor = make_obs() Tcor1 = make_obs() Tcor4 = make_obs() zaxis = np.array([0,0,1]) for _ in range(n): theta = -15 + np.random.random()*30 theta2 = utils.rad_angle(theta) T7 = make_obs() R1 = np.eye(4) R1[0:3,0:3] = rotation_matrix(zaxis,theta2) R4 = np.eye(4) R4[0:3,0:3] = rotation_matrix(zaxis,-theta2) if add_noise: Tcor = T7.dot(T7cor) noise = make_obs(0.01,0.01) T1 = Tcor.dot(R1).dot(Tcor1).dot(noise) noise = make_obs(0.01,0.01) T2 = T1.dot(T12).dot(noise) noise = make_obs(0.01,0.01) T3 = T2.dot(T23).dot(noise) noise = make_obs(0.01,0.01) T4 = Tcor.dot(R4).dot(Tcor4).dot(noise) noise = make_obs(0.01,0.01) T5 = T4.dot(T45).dot(noise) noise = make_obs(0.01,0.01) T6 = T5.dot(T56).dot(noise) noise = make_obs(0.01,0.01) T8 = T7.dot(T78).dot(noise) else: noise = np.eye(4) Tcor = T7.dot(T7cor) T1 = Tcor.dot(R1).dot(Tcor1).dot(noise) T2 = T1.dot(T12).dot(noise) T3 = T2.dot(T23).dot(noise) T4 = Tcor.dot(R4).dot(Tcor4).dot(noise) T5 = T4.dot(T45).dot(noise) T6 = T5.dot(T56).dot(noise) T8 = T7.dot(T78).dot(noise) # Randomly sample a few until done maybe? tfms = {1:T1,2:T2,3:T3,4:T4,5:T5,6:T6,7:T7,8:T8} gc.update_groups_from_observations(masterGraph, tfms, theta) init = np.zeros(36) init[0:12] = nlg.inv(T78).dot(T7cor)[0:3,:].reshape(12,order='F') init[12:24] = nlg.inv(Tcor4)[0:3,:].reshape(12,order='F') init[24:36] = nlg.inv(Tcor1)[0:3,:].reshape(12,order='F') G_opt = gc.compute_relative_transforms(masterGraph, 5)#, init) # 7,8 markers group = G_opt.node["master"] G = group["graph"] print "T78" print "Original:" print T78 print "Calculated:" print G.edge[7][8]['tfm'], '\n' # 1,2,3 markers group = G_opt.node["l"] G = group["graph"] print "T12" print "Original:" print T12 print "Calculated:" print G.edge[1][2]['tfm'], '\n' print "T23" print "Original:" print T23 print "Calculated:" print G.edge[2][3]['tfm'], '\n' print "T31" print "Original:" print nlg.inv(T12.dot(T23)) print "Calculated:" print G.edge[3][1]['tfm'], '\n' # 4,5,6 markers group = G_opt.node["r"] G = group["graph"] print "T45" print "Original:" print T45 print "Calculated:" print G.edge[4][5]['tfm'], '\n' print "T56" print "Original:" print T56 print "Calculated:" print G.edge[5][6]['tfm'], '\n' print "T64" print "Original:" print nlg.inv(T45.dot(T56)) print "Calculated:" print G.edge[6][4]['tfm'], '\n' # Let's try random angles: theta = -15 + np.random.random()*30 theta2 = utils.rad_angle(theta) R1 = np.eye(4) R1[0:3,0:3] = rotation_matrix(zaxis,theta2) print "T71" print "Original:" print T7cor.dot(R1).dot(Tcor1) print "Calculated:" print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n' print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n' print "Difference between cors" T7cor_calib = G_opt.node["master"]["graph"].edge[7]["cor"]["tfm"] print nlg.inv(T7cor).dot(T7cor_calib)
def test_gc3 (n, add_noise=False): ## # Markers 1,2,3 are on l finger # Markers 4,5,6 are on r finger # 7 and 8 are on the middle ## ### Initializing everything # create calib info calib_info = {"master": {"ar_markers":[7,8], "master_group":True, "angle_scale":0}, "l": {"ar_markers":[1,2,3], "angle_scale":1}, "r": {"ar_markers":[4,5,6], "angle_scale":-1}} gripper_calib = gc.GripperCalibrator(None, calib_info=calib_info) gripper_calib.initialize_calibration(fake_data=True) T12 = make_obs() T23 = make_obs() T45 = make_obs() T56 = make_obs() T78 = make_obs() T7cor = make_obs() Tcor1 = make_obs() Tcor4 = make_obs() zaxis = np.array([0,0,1]) for _ in range(n): theta = -15 + np.random.random()*30 theta2 = utils.rad_angle(theta) T7 = make_obs() R1 = np.eye(4) R1[0:3,0:3] = rotation_matrix(zaxis,theta2) R4 = np.eye(4) R4[0:3,0:3] = rotation_matrix(zaxis,-theta2) if add_noise: Tcor = T7.dot(T7cor) noise = make_obs(0.01,0.01) T1 = Tcor.dot(R1).dot(Tcor1).dot(noise) noise = make_obs(0.01,0.01) T2 = T1.dot(T12).dot(noise) noise = make_obs(0.01,0.01) T3 = T2.dot(T23).dot(noise) noise = make_obs(0.01,0.01) T4 = Tcor.dot(R4).dot(Tcor4).dot(noise) noise = make_obs(0.01,0.01) T5 = T4.dot(T45).dot(noise) noise = make_obs(0.01,0.01) T6 = T5.dot(T56).dot(noise) noise = make_obs(0.01,0.01) T8 = T7.dot(T78).dot(noise) else: noise = np.eye(4) Tcor = T7.dot(T7cor) T1 = Tcor.dot(R1).dot(Tcor1).dot(noise) T2 = T1.dot(T12).dot(noise) T3 = T2.dot(T23).dot(noise) T4 = Tcor.dot(R4).dot(Tcor4).dot(noise) T5 = T4.dot(T45).dot(noise) T6 = T5.dot(T56).dot(noise) T8 = T7.dot(T78).dot(noise) # Randomly sample a few until done maybe? tfms = {1:T1,2:T2,3:T3,4:T4,5:T5,6:T6,7:T7,8:T8} gc.update_groups_from_observations(gripper_calib.masterGraph, tfms, theta) init = np.zeros(36) init[0:12] = nlg.inv(T78).dot(T7cor)[0:3,:].reshape(12,order='F') init[12:24] = nlg.inv(Tcor4)[0:3,:].reshape(12,order='F') init[24:36] = nlg.inv(Tcor1)[0:3,:].reshape(12,order='F') gripper_calib.calibrated = gripper_calib.finish_calibration() G_opt = gripper_calib.transform_graph # 7,8 markers group = G_opt.node["master"] G = group["graph"] print "T78" print "Original:" print T78 print "Calculated:" print G.edge[7][8]['tfm'], '\n' # 1,2,3 markers group = G_opt.node["l"] G = group["graph"] print "T12" print "Original:" print T12 print "Calculated:" print G.edge[1][2]['tfm'], '\n' print "T23" print "Original:" print T23 print "Calculated:" print G.edge[2][3]['tfm'], '\n' print "T31" print "Original:" print nlg.inv(T12.dot(T23)) print "Calculated:" print G.edge[3][1]['tfm'], '\n' # 4,5,6 markers group = G_opt.node["r"] G = group["graph"] print "T45" print "Original:" print T45 print "Calculated:" print G.edge[4][5]['tfm'], '\n' print "T56" print "Original:" print T56 print "Calculated:" print G.edge[5][6]['tfm'], '\n' print "T64" print "Original:" print nlg.inv(T45.dot(T56)) print "Calculated:" print G.edge[6][4]['tfm'], '\n' # Let's try random angles: theta = -15 + np.random.random()*30 theta2 = utils.rad_angle(theta) R1 = np.eye(4) R1[0:3,0:3] = rotation_matrix(zaxis,theta2) print "Theta: ",theta print "T71" print "Original:" print T7cor.dot(R1).dot(Tcor1) print "Calculated:" print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n' print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n' theta = -15 + np.random.random()*30 theta2 = utils.rad_angle(theta) R1 = np.eye(4) R1[0:3,0:3] = rotation_matrix(zaxis,theta2) print "Theta: ",theta print "T71" print "Original:" print T7cor.dot(R1).dot(Tcor1) print "Calculated:" print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n' print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n' print "Difference between cors" T7cor_calib = G_opt.node["master"]["graph"].edge[7]["cor"]["tfm"] print nlg.inv(T7cor).dot(T7cor_calib) return gripper_calib
def view_hydra_demo_on_rviz (demo_type, demo_name, freq, speed, prompt=False, verbose=False): """ Uses hydra_only.data for the segment to quickly visualize the demo. @demo_type, @demo_name: demo identification. @freq: basically measure of fine-ness of timesteps. @speed: how fast to replay demo. @prompt: does the user hit enter after each time step? """ demo_dir = osp.join(demo_files_dir, demo_type, demo_name) bag_file = osp.join(demo_dir, demo_names.bag_name) data_file = osp.join(demo_dir, demo_names.hydra_data_name) calib_file = osp.join(demo_dir, demo_names.calib_name) with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh) if not osp.isfile(data_file): yellowprint("%s does not exist for this demo. Extracting now."%demo_names.hydra_data_name) ed.save_hydra_only(demo_type, demo_name) with open(data_file, 'r') as fh: dat = cp.load(fh) # get grippers used grippers = [key for key in dat.keys() if key in 'lr'] # data rgbd_dirs = {cam:osp.join(demo_dir,demo_names.video_dir%cam) for cam in cam_types if cam_types[cam] == 'rgbd'} cam_frames = {cam:'/camera%i_rgb_optical_frame'%cam for cam in rgbd_dirs} tfm_pubs = {} hydra_dat = {} pot_dat = {} _, hydra_dat['l'], pot_dat['l'] = load_data(data_file, 'l', freq, speed, hydra_only=True) _, hydra_dat['r'], pot_dat['r'] = load_data(data_file, 'r', freq, speed, hydra_only=True) tmin, _, nsteps = relative_time_streams(hydra_dat.values() + pot_dat.values(), freq, speed) if rospy.get_name() == "/unnamed": rospy.init_node("visualize_demo") ## publishers for unfiltered-data: for lr in grippers: tfm_pubs[lr] = rospy.Publisher('/%s_hydra_estimate'%(lr), PoseStamped) ## get the point-cloud stream pc_strms = {cam:streamize_rgbd_pc(rgbd_dirs[cam], cam_frames[cam], freq, tstart=tmin,speed=speed,verbose=verbose) for cam in rgbd_dirs} pc_pubs = {cam:rospy.Publisher('/point_cloud%i'%cam, PointCloud2) for cam in rgbd_dirs} cam_tfms = get_cam_transforms (calib_file, len(cam_types)) for cam in rgbd_dirs: if cam != 1: publish_static_tfm(cam_frames[1], cam_frames[cam], cam_tfms[cam]) sleeper = rospy.Rate(freq) T_far = np.eye(4) T_far[0:3,3] = [10,10,10] handles = [] dat_snext = {lr:{} for lr in grippers} for lr in grippers: dat_snext[lr]['h'] = stream_soft_next(hydra_dat[lr]) dat_snext[lr]['pot'] = stream_soft_next(pot_dat[lr]) prev_ang = {'l': 0, 'r': 0} for i in xrange(nsteps): if prompt: raw_input("Hit enter when ready.") if verbose: print "Time stamp: ", tmin+(0.0+i*speed)/freq ## show the point-cloud: for cam in pc_strms: try: pc = pc_strms[cam].next() if pc is not None: if verbose: print "pc%i ts:"%cam, pc.header.stamp.to_sec() pc.header.stamp = rospy.Time.now() pc_pubs[cam].publish(pc) else: if verbose: print "pc%i ts:"%cam,None except StopIteration: pass ests = {} tfms = [] ang_vals = [] for lr in grippers: ests[lr] = dat_snext[lr]['h']() ang_val = dat_snext[lr]['pot']() if ang_val != None and not np.isnan(ang_val): prev_ang[lr] = ang_val ang_val = ang_val else: ang_val = prev_ang[lr] ang_val *= 2 if ests[lr] is None: tfms.append(T_far) else: tfms.append(ests[lr]) ang_vals.append(rad_angle(ang_val)) handles = draw_trajectory(cam_frames[1], tfms, color=(1,1,0,1), open_fracs=ang_vals) for lr in grippers: if ests[lr] is not None: tfm_pubs[lr].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(ests[lr]), cam_frames[1])) sleeper.sleep() empty_cloud = PointCloud2() for cam in pc_pubs: pc_pubs[cam].publish(empty_cloud)
def view_tracking_on_rviz(demo_type, demo_name, tps_model_fname, freq=30.0, speed=1.0, use_smoother=True, prompt=False, verbose=False): """ Visualizes demo after kalman tracking/smoothing on rviz. @demo_type, @demo_name: demo identification. @freq: basically measure of fine-ness of timesteps. @speed: how fast to replay demo. @main: which sensor to display the marker for @prompt: does the user hit enter after each time step? """ demo_dir = osp.join(demo_files_dir, demo_type, demo_name) bag_file = osp.join(demo_dir, demo_names.bag_name) traj_file = osp.join(demo_dir, demo_names.traj_name) calib_file = osp.join(demo_dir, demo_names.calib_name) with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh) if not osp.isfile(traj_file): yellowprint("%s does not exist for this demo. Running kalman filter/smoother now with default args."%demo_names.traj_name) data_file = osp.join(demo_dir, demo_names.data_name) if not osp.isfile(data_file): yellowprint("%s does not exist for this demo. Extracting now."%demo_names.data_name) ed.save_observations_rgbd(demo_type, demo_name) filter_traj(demo_dir, tps_model_fname=tps_model_fname, save_tps=True, do_smooth=True, plot='', block=False) with open(traj_file, 'r') as fh: traj = cp.load(fh) # get grippers used grippers = traj.keys() if rospy.get_name() == "/unnamed": rospy.init_node("visualize_demo") # data rgbd_dirs = {cam:osp.join(demo_dir,demo_names.video_dir%cam) for cam in cam_types if cam_types[cam] == 'rgbd'} pc_pubs = {cam:rospy.Publisher('/point_cloud%i'%cam, PointCloud2) for cam in rgbd_dirs} cam_frames = {cam:'/camera%i_rgb_optical_frame'%cam for cam in rgbd_dirs} cam_tfms = get_cam_transforms (calib_file, len(cam_types)) for cam in rgbd_dirs: if cam != 1: publish_static_tfm(cam_frames[1], cam_frames[cam], cam_tfms[cam]) # Remove segment "done", it is just a single frame segs = sorted(traj[grippers[0]].keys()) if 'done' in segs: segs.remove('done') sleeper = rospy.Rate(freq) T_far = np.eye(4) T_far[0:3,3] = [10,10,10] for seg in segs: if prompt: raw_input("Press enter for segment %s."%seg) else: yellowprint("Segment %s beginning."%seg) time.sleep(1) # Initializing data streams: traj_strms = {} pot_strms = {} tfms_key = 'tfms_s' if use_smoother else 'tfms' for lr in grippers: traj_strms[lr] = streamize(traj[lr][seg][tfms_key], traj[lr][seg]['stamps'], freq, avg_transform, speed=speed) # HACK pot_strms[lr] = streamize(traj[lr][seg]['pot_angles'][:len(traj[lr][seg]['stamps'])], traj[lr][seg]['stamps'], freq, np.mean, speed=speed) tmin, tmax, nsteps = relative_time_streams(traj_strms.values() + pot_strms.values(), freq, speed) pc_strms = {cam:streamize_rgbd_pc(rgbd_dirs[cam], cam_frames[cam], freq, tstart=tmin, tend=tmax,speed=speed,verbose=verbose) for cam in rgbd_dirs} prev_ang = {'l': 0, 'r': 0} dat_snext = {lr:{} for lr in grippers} for lr in grippers: dat_snext[lr]['traj'] = stream_soft_next(traj_strms[lr]) dat_snext[lr]['pot'] = stream_soft_next(pot_strms[lr]) for i in xrange(nsteps): if prompt: raw_input("Hit enter when ready.") if verbose: print "Time stamp: ", tmin+(0.0+i*speed)/freq ## show the point-cloud: found_pc = False for cam in pc_strms: try: pc = pc_strms[cam].next() if pc is not None: if verbose: print "pc%i ts:"%cam, pc.header.stamp.to_sec() pc.header.stamp = rospy.Time.now() pc_pubs[cam].publish(pc) found_pc = True else: if verbose: print "pc%i ts:"%cam,None except StopIteration: pass tfms = [] ang_vals = [] for lr in grippers: if lr == 'r': continue tfm = dat_snext[lr]['traj']() ang_val = dat_snext[lr]['pot']() if ang_val != None and not np.isnan(ang_val): prev_ang[lr] = ang_val ang_val = ang_val else: ang_val = prev_ang[lr] ang_val *= 2 if tfm is None: tfms.append(T_far) else: tfms.append(tfm) ang_vals.append(rad_angle(ang_val)) handles = draw_trajectory(cam_frames[cam], tfms, color=(1,0,0,1), open_fracs=ang_vals) time.sleep(1.0/freq)
def view_demo_on_rviz(demo_type, demo_name, freq, speed=1.0, main='h', prompt=False, verbose=False): """ Visualizes recorded demo on rviz (without kalman filter/smoother data). @demo_type, @demo_name: demo identification. @freq: basically measure of fine-ness of timesteps. @speed: how fast to replay demo. @main: which sensor to display the marker for @prompt: does the user hit enter after each time step? """ demo_dir = osp.join(demo_files_dir, demo_type, demo_name) bag_file = osp.join(demo_dir, demo_names.bag_name) data_file = osp.join(demo_dir, demo_names.data_name) calib_file = osp.join(demo_dir, demo_names.calib_name) with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh) if not osp.isfile(data_file): yellowprint("%s does not exist for this demo. Extracting now."%demo_names.data_name) ed.save_observations_rgbd(demo_type, demo_name) with open(data_file, 'r') as fh: dat = cp.load(fh) # get grippers used grippers = [key for key in dat.keys() if key in 'lr'] # data rgbd_dirs = {cam:osp.join(demo_dir,demo_names.video_dir%cam) for cam in cam_types if cam_types[cam] == 'rgbd'} cam_frames = {cam:'/camera%i_rgb_optical_frame'%cam for cam in rgbd_dirs} tfm_pubs = {} cam_dat = {} hydra_dat = {} pot_dat = {} _, cam_dat['l'], hydra_dat['l'], pot_dat['l'] = load_data(data_file, 'l', freq, speed) _, cam_dat['r'], hydra_dat['r'], pot_dat['r'] = load_data(data_file, 'r', freq, speed) all_cam_strms = [] for lr in 'lr': for cam in cam_dat[lr].keys(): all_cam_strms.append(cam_dat[lr][cam]['stream']) tmin, _, nsteps = relative_time_streams(hydra_dat.values() + pot_dat.values() + all_cam_strms, freq, speed) if rospy.get_name() == "/unnamed": rospy.init_node("visualize_demo") ## publishers for unfiltered-data: for lr in grippers: tfm_pubs[lr] = {} for cam in cam_types: tfm_pubs[lr][cam] = rospy.Publisher('/%s_ar%i_estimate'%(lr,cam), PoseStamped) tfm_pubs[lr]['h'] = rospy.Publisher('/%s_hydra_estimate'%(lr), PoseStamped) ## get the point-cloud stream pc_strms = {cam:streamize_rgbd_pc(rgbd_dirs[cam], cam_frames[cam], freq, tstart=tmin,speed=speed,verbose=verbose) for cam in rgbd_dirs} pc_pubs = {cam:rospy.Publisher('/point_cloud%i'%cam, PointCloud2) for cam in rgbd_dirs} # import IPython # IPython.embed() cam_tfms = get_cam_transforms (calib_file, len(cam_types)) for cam in rgbd_dirs: if cam != 1: publish_static_tfm(cam_frames[1], cam_frames[cam], cam_tfms[cam]) sleeper = rospy.Rate(freq) T_far = np.eye(4) T_far[0:3,3] = [10,10,10] handles = [] prev_ang = {'l': 0, 'r': 0} dat_snext = {lr:{} for lr in grippers} for lr in grippers: dat_snext[lr]['h'] = stream_soft_next(hydra_dat[lr]) dat_snext[lr]['pot'] = stream_soft_next(pot_dat[lr]) for cam in cam_types: dat_snext[lr][cam] = stream_soft_next(cam_dat[lr][cam]['stream']) for i in xrange(nsteps): if prompt: raw_input("Hit enter when ready.") if verbose: print "Time stamp: ", tmin+(0.0+i*speed)/freq ## show the point-cloud: found_pc = False for cam in pc_strms: try: pc = pc_strms[cam].next() if pc is not None: if verbose: print "pc%i ts:"%cam, pc.header.stamp.to_sec() pc.header.stamp = rospy.Time.now() pc_pubs[cam].publish(pc) found_pc = True else: if verbose: print "pc%i ts:"%cam,None except StopIteration: pass next_est = {lr:{} for lr in grippers} tfms = [] ang_vals = [] if main != 'h': main = int(main) for lr in grippers: next_est[lr]['h'] = dat_snext[lr]['h']() for cam in cam_types: next_est[lr][cam] = dat_snext[lr][cam]() ang_val = dat_snext[lr]['pot']() if ang_val != None and not np.isnan(ang_val): prev_ang[lr] = ang_val ang_val = ang_val else: ang_val = prev_ang[lr] ang_val *= 2 tfm = next_est[lr][main] if tfm is None: tfms.append(T_far) else: tfms.append(tfm) ang_vals.append(rad_angle(ang_val)) handles = draw_trajectory(cam_frames[1], tfms, color=(1,1,0,1), open_fracs=ang_vals) for lr in grippers: for m,est in next_est[lr].items(): if est != None: tfm_pubs[lr][m].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(est), cam_frames[1])) else: tfm_pubs[lr][m].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(T_far), cam_frames[1])) sleeper.sleep()