def process_trialobj( trial_num, obj_num, servo_yn ): obj_name = tdb[obj_num][0] tname = obj_name.replace( ' ', '' ) loc = (trial_num + obj_num) % 9 loc_name = pts[loc][0] loc_pos = np.array(pts[loc][1]) # Tag ground-truth location fname_prefix = '/home/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/search_cap/' servo_fname = fname_prefix servo_fname += 'search_aware_home/' servo_fname += 'woot_150_'+str(trial_num)+'_tag_'+obj_name.replace(' ','')+'_servo.pkl' pf_search = servo_fname.replace('_servo.pkl', '_pf_search.pkl') pf_servo = servo_fname.replace('_servo.pkl', '_pf_servo.pkl') # Search only search_reads_fname = fname_prefix search_reads_fname += 'search_aware_home/woot_150_'+str(trial_num)+'_reads.pkl' f = open( search_reads_fname, 'r' ) summary_search = pkl.load( f ) f.close() pos_readings_search = sum([ True for p in summary_search if p.read.rssi != -1 and p.read.tagID == obj_name ]) tot_readings_search = len( summary_search ) if pos_readings_search == 0: # No results! print '\t No results for this instance.' res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings_search, 'tot_readings':tot_readings_search, 'best_pos': '--', 'orient_est': '--', 'dxy': '--', 'dtheta': '--', 'servo_yn': servo_yn, 'other': { 'w_mass': 0.0, # specific to pf 'orient_fit': 0.0 }} return False, res, None f = open( pf_search ) # load the particle set from Search data. Will overwrite if using search plus servo p_set_loaded = pkl.load( f ) f.close() pos_readings = pos_readings_search tot_readings = tot_readings_search # If this is Search PLUS servo... if servo_yn: # update the stats for pos reads f = open( servo_fname, 'r' ) summary_servo = pkl.load( f ) f.close() pos_readings_servo = sum([ True for p in summary_servo if p.read.rssi != -1 and p.read.tagID == obj_name ]) tot_readings_servo = len( summary_servo ) pos_readings = pos_readings_search + pos_readings_servo tot_readings = tot_readings_search + tot_readings_servo # use the particle set from the SERVO data f = open( pf_servo ) p_set_loaded = pkl.load( f ) f.close() # print '\t Positive Reads: %d of %d (%2.1f)' % ( pos_readings, # tot_readings, # 100.0 * pos_readings / tot_readings ) # maxw = np.max( p_set_loaded[:,2] ) # minw = np.min( p_set_loaded[:,2] ) # w_normed = 1.0 * ( p_set_loaded[:,2] - minw ) / ( maxw - minw ) # # Only keep particles in the top 98% of likelihoods (250 / 255). This # # makes the selection only consider locations where the probability # # mass is actually located. (These are the ones displayed in the # # screen captures!) # p_set = p_set_loaded[ np.where( w_normed > 0.02 )[0] ] # print 'Shape: ', p_set.shape # # print 'p_set size (pre-cap): ', p_set.shape # # p_set_precap = np.copy( p_set ) # # p_set = p_set[np.argsort(p_set[:,2])][:1000] # Cap the total number: keep at most the top 1000 # # print 'p_set size (pre-cap): ', p_set.shape p_set = np.copy( p_set_loaded ) p_set = p_set[ np.argsort( p_set[:,2] )[::-1] ] # sort by decreasing weight w_norm = p_set[:,2] / np.sum( p_set[:,2] ) w_cum = np.cumsum( w_norm ) # ONLY KEEP top 8000 particles (computation) w_mass = w_cum[:8000][-1] * 100.0 # Ratio of mass in p_set to total: p_set = p_set[:8000] # only keep top 8000 for computational reasons! # # ONLY KEEP particles that are in top 98% of normalized values. (these are the ones displayed) # maxw = np.max( p_set[:,2] ) # minw = np.min( p_set[:,2] ) # w_scaled = 1.0 * ( p_set[:,2] - minw ) / ( maxw - minw ) # p_set = p_set[ np.where( w_scaled > 0.02 )[0] ] # p_set = p_set[:8000] # Only keep top 8000 max # w_mass = w_cum[ p_set.shape[0] ] # print 'p_set size (pre-cap): ', p_set.shape # print w_mass # print '\tShape: ', p_set.shape pf_costmap = '/home/travis/svn/robot1/src/projects/rfid_pf/src/rfid_pf/pf_costmap.pkl' f = open( pf_costmap ) costmap = pkl.load( f ) f.close() cm = costmap[ np.where( costmap[:,2] )[0] ] # Locations where the robot can be located # Determine the score for each possible xy robot location def score_loc( xy ): # The xy location under consideration for the robot mag = np.sqrt( np.sum( np.power( p_set[:,0:2] - xy, 2.0 ), axis = 1)) # || dist || score = mag * p_set[:,2] # || dist || * w's return np.sum( score ) # Compute all the scores for possible robot locations t0 = time.time() pos_scores = [ score_loc( i[0:2] ) for i in cm ] dt = time.time() - t0 # print '\tScore computations per second: ', len( pos_scores ) * 1.0 / dt best_ind = np.argmin( pos_scores ) best_pos = cm[ best_ind ][0:2] # # Calculate the angle that is the mean # # Now that we have best_pos, we need to find the best orientation. # def score_orient( xyw, best_pos ): # returns 1x2 # # xyw is 1x3 # dxy = xyw[0:2] - best_pos # move into best_pose frame (1x2) # dxy_unit = dxy / np.linalg.norm( dxy ) # normalize to unit circle # return dxy_unit * xyw[2] # 1x2; [x_circ => x / |dxy| * w, y_circ...] # so = np.array([ score_orient( i, best_pos ) for i in p_set ]) # so[ np.where(np.isnan( so )) ] = 0.0 # for positions where the particle is one and the same, the norm is 0.0 so score is nan. # x_so = np.sum( so[:,0] ) # y_so = np.sum( so[:,1] ) # orient_est = np.arctan2( y_so, x_so ) # orient_fit = np.sqrt( x_so**2.0 + y_so**2.0 ) / ( np.sum( p_set[:,2] )) # Brute force calculate the angle that yields the minimum |dtheta| theta_hats = np.linspace( -1.0 * np.pi, np.pi, 360, endpoint = False ) theta_hats = np.array([ mu.standard_rad( i ) for i in theta_hats ]) dxy = p_set[:,0:2] - best_pos # put the p_set into the best_pos frame! pset_thetas = np.arctan2( dxy[:,1], dxy[:,0] ) pset_thetas = np.array([ mu.standard_rad( i ) for i in pset_thetas ]) pset_w_normed = p_set[:,2] / np.sum( p_set[:,2] ) def exp_err( th ): errs = np.abs([ mu.standard_rad( i ) for i in th - pset_thetas ]) weighted_errs = pset_w_normed * errs mean_we = np.mean( weighted_errs ) return mean_we theta_hats_res = np.array([ exp_err( i ) for i in theta_hats ]) # rrr = theta_hats # res = theta_hats_res orient_est_ind = np.argmin( theta_hats_res ) orient_est = theta_hats[ orient_est_ind ] # Compute errors: dxy = np.linalg.norm( best_pos - loc_pos[0:2] ) true_theta = np.arctan2( loc_pos[1] - best_pos[1], # y / x loc_pos[0] - best_pos[0] ) dtheta = mu.standard_rad( orient_est - true_theta ) res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings, 'tot_readings':tot_readings, 'best_pos': best_pos, 'orient_est': orient_est, 'dxy': dxy, 'dtheta': dtheta, 'servo_yn': servo_yn, 'other': { 'w_mass': w_mass, # specific to pf 'theta_hats': theta_hats, 'theta_hats_res': theta_hats_res, 'p_set': p_set }} return True, res, np.copy( p_set ) # Was_reads?, results dict, particles (for display)
def process_trialobj( trial_num, obj_num, servo_yn ): obj_name = ahe.tdb[ obj_num ][0] tname = obj_name.replace( ' ', '' ) loc = (trial_num + obj_num) % 9 loc_name = ahe.pts[loc][0] loc_pos = np.array(ahe.pts[loc][1]) # Tag ground-truth location print 'Trial %d with Object %d (%s) at Position %d (%s)' % (trial_num, obj_num, obj_name, loc, loc_name) fname = 'search_aware_home/woot_150_'+str(trial_num)+'_reads.pkl' f = open( fname, 'r' ) summary_search = pkl.load( f ) f.close() pos_readings_search = sum([ True for p in summary_search if p.read.rssi != -1 and p.read.tagID == obj_name ]) tot_readings_search = len( summary_search ) search_positions_fname = 'search_aware_home/woot_150_' + str(trial_num) + '_tag_' + tname + '.yaml' servo_positions_fname = 'search_aware_home/woot_150_' + str(trial_num) + '_tag_' + tname + '_end.txt' servo_fname = 'search_aware_home/woot_150_' + str(trial_num) + '_tag_' + tname + '_servo.pkl' if pos_readings_search == 0: print '\t No results for this instance.' res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings_search, 'tot_readings':tot_readings_search, 'best_pos': '--', 'orient_est': '--', 'dxy': '--', 'dtheta': '--', 'servo_yn': servo_yn, 'other': { 'w_mass': 0.0, # specific to pf 'orient_fit': 0.0 }} return False, res, None # All fnames should be defined if we got here! f = open( search_positions_fname ) y = yaml.load( f ) f.close() # "Best" Location determined by search efq = tft.euler_from_quaternion search_theta = efq( [ y['pose']['orientation']['x'], y['pose']['orientation']['y'], y['pose']['orientation']['z'], y['pose']['orientation']['w'] ])[-1] search_pos = np.array([ y['pose']['position']['x'], y['pose']['position']['y'], y['pose']['position']['z'] ]) search_true_theta = np.arctan2( loc_pos[1] - search_pos[1], # y / x loc_pos[0] - search_pos[0] ) search_theta_diff = mu.standard_rad( search_theta - search_true_theta ) # search_pos_diff = np.linalg.norm( search_pos - loc_pos ) search_pos_diff = np.linalg.norm( search_pos[0:2] - loc_pos[0:2] ) # Should we return the search only results? if not servo_yn: res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings_search, 'tot_readings':tot_readings_search, 'best_pos': search_pos, 'orient_est': search_theta, 'dxy': search_pos_diff, 'dtheta': search_theta_diff, 'servo_yn': servo_yn } return True, res, None # Else... compute the SEARCH PLUS SERVO results # update the stats for pos reads f = open( servo_fname, 'r' ) summary_servo = pkl.load( f ) f.close() pos_readings_servo = sum([ True for p in summary_servo if p.read.rssi != -1 and p.read.tagID == obj_name ]) tot_readings_servo = len( summary_servo ) # Location after Servoing f = open( servo_positions_fname ) r = f.readlines() f.close() # ['At time 1313069718.853\n', # '- Translation: [2.811, 1.711, 0.051]\n', # '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n', # ' in RPY [0.005, 0.003, -0.229]\n', # 'At time 1313069719.853\n', # '- Translation: [2.811, 1.711, 0.051]\n', # '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n', # ' in RPY [0.005, 0.002, -0.229]\n'] rpy = r[-1].find('RPY')+3 servo_theta = json.loads( r[-1][rpy:] )[-1] tion = r[-3].find('tion:')+5 servo_pos = np.array(json.loads( r[-3][tion:] )) servo_true_theta = np.arctan2( loc_pos[1] - servo_pos[1], # y / x loc_pos[0] - servo_pos[0] ) servo_theta_diff = mu.standard_rad( servo_theta - servo_true_theta ) # servo_pos_diff = np.linalg.norm( servo_pos - loc_pos ) servo_pos_diff = np.linalg.norm( servo_pos[0:2] - loc_pos[0:2] ) # print '\t Post-Servo Stats:' # print '\t\t Tag-Robot distance err (m): %2.3f' % (servo_pos_diff) # print '\t\t Tag-Robot orient err (deg): %2.3f' % (math.degrees(servo_theta_diff)) # print '\t\t Debug Stats', math.degrees(servo_theta), math.degrees(servo_true_theta), servo_pos, loc_pos res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings_search + pos_readings_servo, 'tot_readings':tot_readings_search + tot_readings_servo, 'best_pos': servo_pos, 'orient_est': servo_theta, 'dxy': servo_pos_diff, 'dtheta': servo_theta_diff, 'servo_yn': servo_yn } return True, res, None return
def exp_err( th ): errs = np.abs([ mu.standard_rad( i ) for i in th - pset_thetas ]) weighted_errs = pset_w_normed * errs mean_we = np.mean( weighted_errs ) return mean_we
def exp_err(th): errs = np.abs([mu.standard_rad(i) for i in th - pset_thetas]) weighted_errs = pset_w_normed * errs mean_we = np.mean(weighted_errs) return mean_we
def process_trialobj(trial_num, obj_num, servo_yn): obj_name = tdb[obj_num][0] tname = obj_name.replace(' ', '') loc = (trial_num + obj_num) % 9 loc_name = pts[loc][0] loc_pos = np.array(pts[loc][1]) # Tag ground-truth location fname_prefix = '/home/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/search_cap/' servo_fname = fname_prefix servo_fname += 'search_aware_home/' servo_fname += 'woot_150_' + str(trial_num) + '_tag_' + obj_name.replace( ' ', '') + '_servo.pkl' pf_search = servo_fname.replace('_servo.pkl', '_pf_search.pkl') pf_servo = servo_fname.replace('_servo.pkl', '_pf_servo.pkl') # Search only search_reads_fname = fname_prefix search_reads_fname += 'search_aware_home/woot_150_' + str( trial_num) + '_reads.pkl' f = open(search_reads_fname, 'r') summary_search = pkl.load(f) f.close() pos_readings_search = sum([ True for p in summary_search if p.read.rssi != -1 and p.read.tagID == obj_name ]) tot_readings_search = len(summary_search) if pos_readings_search == 0: # No results! print '\t No results for this instance.' res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings_search, 'tot_readings': tot_readings_search, 'best_pos': '--', 'orient_est': '--', 'dxy': '--', 'dtheta': '--', 'servo_yn': servo_yn, 'other': { 'w_mass': 0.0, # specific to pf 'orient_fit': 0.0 } } return False, res, None f = open( pf_search ) # load the particle set from Search data. Will overwrite if using search plus servo p_set_loaded = pkl.load(f) f.close() pos_readings = pos_readings_search tot_readings = tot_readings_search # If this is Search PLUS servo... if servo_yn: # update the stats for pos reads f = open(servo_fname, 'r') summary_servo = pkl.load(f) f.close() pos_readings_servo = sum([ True for p in summary_servo if p.read.rssi != -1 and p.read.tagID == obj_name ]) tot_readings_servo = len(summary_servo) pos_readings = pos_readings_search + pos_readings_servo tot_readings = tot_readings_search + tot_readings_servo # use the particle set from the SERVO data f = open(pf_servo) p_set_loaded = pkl.load(f) f.close() # print '\t Positive Reads: %d of %d (%2.1f)' % ( pos_readings, # tot_readings, # 100.0 * pos_readings / tot_readings ) # maxw = np.max( p_set_loaded[:,2] ) # minw = np.min( p_set_loaded[:,2] ) # w_normed = 1.0 * ( p_set_loaded[:,2] - minw ) / ( maxw - minw ) # # Only keep particles in the top 98% of likelihoods (250 / 255). This # # makes the selection only consider locations where the probability # # mass is actually located. (These are the ones displayed in the # # screen captures!) # p_set = p_set_loaded[ np.where( w_normed > 0.02 )[0] ] # print 'Shape: ', p_set.shape # # print 'p_set size (pre-cap): ', p_set.shape # # p_set_precap = np.copy( p_set ) # # p_set = p_set[np.argsort(p_set[:,2])][:1000] # Cap the total number: keep at most the top 1000 # # print 'p_set size (pre-cap): ', p_set.shape p_set = np.copy(p_set_loaded) p_set = p_set[np.argsort(p_set[:, 2])[::-1]] # sort by decreasing weight w_norm = p_set[:, 2] / np.sum(p_set[:, 2]) w_cum = np.cumsum(w_norm) # ONLY KEEP top 8000 particles (computation) w_mass = w_cum[:8000][-1] * 100.0 # Ratio of mass in p_set to total: p_set = p_set[:8000] # only keep top 8000 for computational reasons! # # ONLY KEEP particles that are in top 98% of normalized values. (these are the ones displayed) # maxw = np.max( p_set[:,2] ) # minw = np.min( p_set[:,2] ) # w_scaled = 1.0 * ( p_set[:,2] - minw ) / ( maxw - minw ) # p_set = p_set[ np.where( w_scaled > 0.02 )[0] ] # p_set = p_set[:8000] # Only keep top 8000 max # w_mass = w_cum[ p_set.shape[0] ] # print 'p_set size (pre-cap): ', p_set.shape # print w_mass # print '\tShape: ', p_set.shape pf_costmap = '/home/travis/svn/robot1/src/projects/rfid_pf/src/rfid_pf/pf_costmap.pkl' f = open(pf_costmap) costmap = pkl.load(f) f.close() cm = costmap[np.where( costmap[:, 2])[0]] # Locations where the robot can be located # Determine the score for each possible xy robot location def score_loc(xy): # The xy location under consideration for the robot mag = np.sqrt(np.sum(np.power(p_set[:, 0:2] - xy, 2.0), axis=1)) # || dist || score = mag * p_set[:, 2] # || dist || * w's return np.sum(score) # Compute all the scores for possible robot locations t0 = time.time() pos_scores = [score_loc(i[0:2]) for i in cm] dt = time.time() - t0 # print '\tScore computations per second: ', len( pos_scores ) * 1.0 / dt best_ind = np.argmin(pos_scores) best_pos = cm[best_ind][0:2] # # Calculate the angle that is the mean # # Now that we have best_pos, we need to find the best orientation. # def score_orient( xyw, best_pos ): # returns 1x2 # # xyw is 1x3 # dxy = xyw[0:2] - best_pos # move into best_pose frame (1x2) # dxy_unit = dxy / np.linalg.norm( dxy ) # normalize to unit circle # return dxy_unit * xyw[2] # 1x2; [x_circ => x / |dxy| * w, y_circ...] # so = np.array([ score_orient( i, best_pos ) for i in p_set ]) # so[ np.where(np.isnan( so )) ] = 0.0 # for positions where the particle is one and the same, the norm is 0.0 so score is nan. # x_so = np.sum( so[:,0] ) # y_so = np.sum( so[:,1] ) # orient_est = np.arctan2( y_so, x_so ) # orient_fit = np.sqrt( x_so**2.0 + y_so**2.0 ) / ( np.sum( p_set[:,2] )) # Brute force calculate the angle that yields the minimum |dtheta| theta_hats = np.linspace(-1.0 * np.pi, np.pi, 360, endpoint=False) theta_hats = np.array([mu.standard_rad(i) for i in theta_hats]) dxy = p_set[:, 0:2] - best_pos # put the p_set into the best_pos frame! pset_thetas = np.arctan2(dxy[:, 1], dxy[:, 0]) pset_thetas = np.array([mu.standard_rad(i) for i in pset_thetas]) pset_w_normed = p_set[:, 2] / np.sum(p_set[:, 2]) def exp_err(th): errs = np.abs([mu.standard_rad(i) for i in th - pset_thetas]) weighted_errs = pset_w_normed * errs mean_we = np.mean(weighted_errs) return mean_we theta_hats_res = np.array([exp_err(i) for i in theta_hats]) # rrr = theta_hats # res = theta_hats_res orient_est_ind = np.argmin(theta_hats_res) orient_est = theta_hats[orient_est_ind] # Compute errors: dxy = np.linalg.norm(best_pos - loc_pos[0:2]) true_theta = np.arctan2( loc_pos[1] - best_pos[1], # y / x loc_pos[0] - best_pos[0]) dtheta = mu.standard_rad(orient_est - true_theta) res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings, 'tot_readings': tot_readings, 'best_pos': best_pos, 'orient_est': orient_est, 'dxy': dxy, 'dtheta': dtheta, 'servo_yn': servo_yn, 'other': { 'w_mass': w_mass, # specific to pf 'theta_hats': theta_hats, 'theta_hats_res': theta_hats_res, 'p_set': p_set } } return True, res, np.copy( p_set) # Was_reads?, results dict, particles (for display)
def process_trialobj(trial_num, obj_num, servo_yn): obj_name = ahe.tdb[obj_num][0] tname = obj_name.replace(' ', '') loc = (trial_num + obj_num) % 9 loc_name = ahe.pts[loc][0] loc_pos = np.array(ahe.pts[loc][1]) # Tag ground-truth location print 'Trial %d with Object %d (%s) at Position %d (%s)' % ( trial_num, obj_num, obj_name, loc, loc_name) fname = 'search_aware_home/woot_150_' + str(trial_num) + '_reads.pkl' f = open(fname, 'r') summary_search = pkl.load(f) f.close() pos_readings_search = sum([ True for p in summary_search if p.read.rssi != -1 and p.read.tagID == obj_name ]) tot_readings_search = len(summary_search) search_positions_fname = 'search_aware_home/woot_150_' + str( trial_num) + '_tag_' + tname + '.yaml' servo_positions_fname = 'search_aware_home/woot_150_' + str( trial_num) + '_tag_' + tname + '_end.txt' servo_fname = 'search_aware_home/woot_150_' + str( trial_num) + '_tag_' + tname + '_servo.pkl' if pos_readings_search == 0: print '\t No results for this instance.' res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings_search, 'tot_readings': tot_readings_search, 'best_pos': '--', 'orient_est': '--', 'dxy': '--', 'dtheta': '--', 'servo_yn': servo_yn, 'other': { 'w_mass': 0.0, # specific to pf 'orient_fit': 0.0 } } return False, res, None # All fnames should be defined if we got here! f = open(search_positions_fname) y = yaml.load(f) f.close() # "Best" Location determined by search efq = tft.euler_from_quaternion search_theta = efq([ y['pose']['orientation']['x'], y['pose']['orientation']['y'], y['pose']['orientation']['z'], y['pose']['orientation']['w'] ])[-1] search_pos = np.array([ y['pose']['position']['x'], y['pose']['position']['y'], y['pose']['position']['z'] ]) search_true_theta = np.arctan2( loc_pos[1] - search_pos[1], # y / x loc_pos[0] - search_pos[0]) search_theta_diff = mu.standard_rad(search_theta - search_true_theta) # search_pos_diff = np.linalg.norm( search_pos - loc_pos ) search_pos_diff = np.linalg.norm(search_pos[0:2] - loc_pos[0:2]) # Should we return the search only results? if not servo_yn: res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings_search, 'tot_readings': tot_readings_search, 'best_pos': search_pos, 'orient_est': search_theta, 'dxy': search_pos_diff, 'dtheta': search_theta_diff, 'servo_yn': servo_yn } return True, res, None # Else... compute the SEARCH PLUS SERVO results # update the stats for pos reads f = open(servo_fname, 'r') summary_servo = pkl.load(f) f.close() pos_readings_servo = sum([ True for p in summary_servo if p.read.rssi != -1 and p.read.tagID == obj_name ]) tot_readings_servo = len(summary_servo) # Location after Servoing f = open(servo_positions_fname) r = f.readlines() f.close() # ['At time 1313069718.853\n', # '- Translation: [2.811, 1.711, 0.051]\n', # '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n', # ' in RPY [0.005, 0.003, -0.229]\n', # 'At time 1313069719.853\n', # '- Translation: [2.811, 1.711, 0.051]\n', # '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n', # ' in RPY [0.005, 0.002, -0.229]\n'] rpy = r[-1].find('RPY') + 3 servo_theta = json.loads(r[-1][rpy:])[-1] tion = r[-3].find('tion:') + 5 servo_pos = np.array(json.loads(r[-3][tion:])) servo_true_theta = np.arctan2( loc_pos[1] - servo_pos[1], # y / x loc_pos[0] - servo_pos[0]) servo_theta_diff = mu.standard_rad(servo_theta - servo_true_theta) # servo_pos_diff = np.linalg.norm( servo_pos - loc_pos ) servo_pos_diff = np.linalg.norm(servo_pos[0:2] - loc_pos[0:2]) # print '\t Post-Servo Stats:' # print '\t\t Tag-Robot distance err (m): %2.3f' % (servo_pos_diff) # print '\t\t Tag-Robot orient err (deg): %2.3f' % (math.degrees(servo_theta_diff)) # print '\t\t Debug Stats', math.degrees(servo_theta), math.degrees(servo_true_theta), servo_pos, loc_pos res = { 'loc': loc, 'obj_num': obj_num, 'trial_num': trial_num, 'pos_readings': pos_readings_search + pos_readings_servo, 'tot_readings': tot_readings_search + tot_readings_servo, 'best_pos': servo_pos, 'orient_est': servo_theta, 'dxy': servo_pos_diff, 'dtheta': servo_theta_diff, 'servo_yn': servo_yn } return True, res, None return
f.close() # "Best" Location determined by search efq = tft.euler_from_quaternion search_theta = efq( [ y['pose']['orientation']['x'], y['pose']['orientation']['y'], y['pose']['orientation']['z'], y['pose']['orientation']['w'] ])[-1] search_pos = np.array([ y['pose']['position']['x'], y['pose']['position']['y'], y['pose']['position']['z'] ]) search_true_theta = np.arctan2( loc_pos[1] - search_pos[1], # y / x loc_pos[0] - search_pos[0] ) search_theta_diff = mu.standard_rad( search_theta - search_true_theta ) search_pos_diff = np.linalg.norm( search_pos - loc_pos ) print '\t Post-Search Stats:' print '\t\t Tag-Robot distance err (m): %2.3f' % (search_pos_diff) print '\t\t Tag-Robot orient err (deg): %2.3f' % (math.degrees(search_theta_diff)) print '\t\t Debug Stats', math.degrees(search_theta), math.degrees(search_true_theta), search_pos, loc_pos # Location after Servoing f = open( servo_fname ) r = f.readlines() f.close() # ['At time 1313069718.853\n', # '- Translation: [2.811, 1.711, 0.051]\n', # '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n', # ' in RPY [0.005, 0.003, -0.229]\n',