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
00023
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
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