test02.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('hai_sandbox')
00002 import rospy
00003 import hrl_lib.util as hru
00004 import pylab as pb
00005 import numpy as np
00006 import itertools as it
00007 
00008 def contact_mat(contact_msgs)
00009     start_time = contact_msgs[0].header.stamp
00010     times = np.array([c.header.stamp for c in contact_msgs]) - start_time
00011     left, right = zip(*[[list(c.l_finger_tip), list(c.r_finger_tip)] for c in contact_msgs])
00012     
00013     left = np.matrix(left).T
00014     right = np.matrix(right).T
00015     return left, right, times
00016 
00017 
00018 def find_contact_times(left_mat, right_mat, times):
00019     left_mat = left_mat - left_mat[:, 0] 
00020     right_mat = right_mat - right_mat[:,0]
00021     
00022     #When/where did contact happen? 
00023     #TODO: we are assuming just one finger of one arm here!
00024     loc_r, time_c = np.where(np.abs(left_mat) > 250)
00025     times_contact = times[time_c.A1]
00026 
00027     return loc_r, times_contact
00028    
00029 
00030 contact_msgs = hru.load_pickle('contact.pkl')
00031 left_mat, right_mat, times = contact_mat(contact_msgs)
00032 contact_loc, times_contact = find_contact_times(left_mat, right_mat, times)
00033 
00034 #Plot readings
00035 pb.figure()
00036 x = range(left.shape[1])
00037 for i in range(left.shape[0]):
00038     pb.plot(times, right_mat[i,:].T.A1, label=str(i))
00039 pb.legend()
00040 pb.show()
00041 
00042 
00043 
00044 
00045 
00046 


hai_sandbox
Author(s): Hai Nguyen
autogenerated on Wed Nov 27 2013 11:46:56