Go to the documentation of this file.00001
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
00017
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
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
00031
00032
00033
00034
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