log_ft_data.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib; roslib.load_manifest('modeling_forces')
00004 import rospy
00005 import force_torque.FTClient as ftc
00006 import hrl_lib.util as ut
00007 
00008 from std_msgs.msg import Empty
00009 
00010 import time
00011 import numpy as np
00012 import glob
00013 
00014 
00015 ##
00016 # make an appropriate plot.
00017 # @param d - dictionary saved in the pkl
00018 def plot_ft(d):
00019     ft_list = d['ft_list']
00020     time_list = d['time_list']
00021     ft_mat = np.matrix(ft_list).T # 6xN np matrix
00022     force_mat = ft_mat[0:3, :]
00023 
00024     tarray = np.array(time_list)
00025     print 'average rate', 1. / np.mean(tarray[1:] - tarray[:-1])
00026     time_list = (tarray - tarray[0]).tolist()
00027     print len(time_list)
00028     
00029     force_mag_l = ut.norm(force_mat).A1.tolist()
00030     #for i,f in enumerate(force_mag_l):
00031     #    if f > 15:
00032     #        break
00033     #force_mag_l = force_mag_l[i:]
00034     #time_list = time_list[i:]
00035 
00036     mpu.plot_yx(force_mag_l, time_list, axis=None, label='Force Magnitude',
00037                 xlabel='Time(seconds)', ylabel='Force Magnitude (N)',
00038                 color = mpu.random_color())
00039 
00040 def got_trigger_cb(data, d):
00041     d['flag'] = True
00042 
00043 
00044 if __name__ == '__main__':
00045 
00046     import optparse
00047     p = optparse.OptionParser()
00048     p.add_option('-l', '--log', action='store_true', dest='log', help='log FT data')
00049     p.add_option('-p', '--plot', action='store_true', dest='plot', help='plot FT data')
00050     p.add_option('-r', '--ros', action='store_true', dest='ros',
00051                  help='start and stop logging messages over ROS.')
00052     p.add_option('-a', '--all', action='store_true', dest='all',
00053                  help='plot all the pkls in a single plot')
00054     p.add_option('-f', '--fname', action='store', default='',
00055                  type='string', dest='fname', help='pkl with logged data')
00056     
00057     opt, args = p.parse_args()
00058 
00059     if opt.log:
00060         client = ftc.FTClient('/force_torque_ft2')
00061         l = []
00062         time_list = []
00063 
00064         if opt.ros:
00065             topic_name_cb = '/ftlogger/trigger'
00066             got_trigger_dict = {'flag': False}
00067             rospy.Subscriber(topic_name_cb, Empty, got_trigger_cb,
00068                              got_trigger_dict)
00069 
00070             while got_trigger_dict['flag'] == False:
00071                 time.sleep(0.05)
00072 
00073             got_trigger_dict['flag'] = False
00074             while not rospy.is_shutdown():
00075                 ft, t_msg = client.read(fresh=True, with_time_stamp=True)
00076                 if ft != None:
00077                     l.append(ft.A1.tolist())
00078                     time_list.append(t_msg)
00079                 if got_trigger_dict['flag'] == True:
00080                     break
00081                 time.sleep(1/1000.0)
00082         else:
00083             print 'Sleeping for 5 seconds.'
00084             time.sleep(5.)
00085             print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
00086             print 'BEGIN'
00087             print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
00088             client.bias()
00089 
00090             t_start = time.time()
00091             t_now = time.time()
00092             logging_time = 5.
00093 
00094             while (t_now - t_start) < logging_time:
00095                 ft, t_msg = client.read(fresh=True, with_time_stamp=True)
00096                 t_now = time.time()
00097                 if ft != None:
00098                     l.append(ft.A1.tolist())
00099                     time_list.append(t_msg)
00100                 time.sleep(1/1000.0)
00101 
00102         print 'saving the pickle'
00103         d = {}
00104         d['ft_list'] = l
00105         d['time_list'] = time_list
00106 
00107         fname = 'ft_log_'+ut.formatted_time()+'.pkl'
00108         ut.save_pickle(d, fname)
00109 
00110 
00111     if opt.plot:
00112         import matplotlib_util.util as mpu
00113 
00114         if opt.fname == '' and (not opt.all):
00115             raise RuntimeError('need a pkl name (-f or --fname) or --all')
00116 
00117         if opt.all:
00118             fname_list = glob.glob('ft_log*.pkl')
00119         else:
00120             fname_list = [opt.fname]
00121 
00122         for fname in fname_list:
00123             d = ut.load_pickle(fname)
00124             plot_ft(d)
00125 
00126         mpu.legend()
00127         mpu.show()
00128 
00129 


2010_biorob_everyday_mechanics
Author(s): Advait Jain, Hai Nguyen, Charles C. Kemp (Healthcare Robotics Lab, Georgia Tech)
autogenerated on Wed Nov 27 2013 11:58:43