00001
00002 import math, numpy as np
00003 import glob
00004
00005 import roslib; roslib.load_manifest('hrl_tilting_hokuyo')
00006 import hrl_tilting_hokuyo.display_3d_mayavi as d3m
00007
00008 import matplotlib_util.util as mpu
00009 import hrl_lib.util as ut
00010
00011
00012 if __name__ == '__main__':
00013
00014 import optparse
00015 p = optparse.OptionParser()
00016 p.add_option('-f', '--fname', action='store', default='',
00017 type='string', dest='fname', help='pkl with logged data')
00018 p.add_option('-d', '--dir', action='store', default='',
00019 type='string', dest='dir', help='directory with logged data')
00020 opt, args = p.parse_args()
00021
00022 if opt.dir != '':
00023 poses_pkl = glob.glob(opt.dir + '/poses_dict*.pkl')[0]
00024 d = ut.load_pickle(poses_pkl)
00025 elif opt.fname != '':
00026 d = ut.load_pickle(opt.fname)
00027 else:
00028 raise RuntimeError('need either -d or -f')
00029
00030 hand_list = d['hand']['pos_list']
00031 hand_rot = d['hand']['rot_list']
00032 if hand_list != []:
00033 hand_mat = np.column_stack(hand_list)
00034 print 'hand_mat.shape', hand_mat.shape
00035 d3m.plot_points(hand_mat, color = (1.,0.,0.), mode='sphere')
00036
00037 directions_x = (np.row_stack(hand_rot)[:,0]).T.reshape(len(hand_rot), 3).T
00038 directions_z = (np.row_stack(hand_rot)[:,2]).T.reshape(len(hand_rot), 3).T
00039
00040
00041
00042
00043
00044
00045
00046
00047 d3m.plot_normals(hand_mat, directions_x, color=(1.,0,0.))
00048 d3m.plot_normals(hand_mat, directions_z, color=(0.,0,1.))
00049
00050 mechanism_list = d['mechanism']['pos_list']
00051 mechanism_rot = d['mechanism']['rot_list']
00052
00053 if mechanism_list != []:
00054 mechanism_mat = np.column_stack(mechanism_list)
00055 print 'mechanism_mat.shape', mechanism_mat.shape
00056 d3m.plot_points(mechanism_mat, color = (0.,0.,1.), mode='sphere')
00057
00058 c = np.row_stack(mechanism_rot)[:,2]
00059 directions = c.T.reshape(len(mechanism_rot), 3).T
00060 print 'directions.shape', directions.shape
00061 print 'mechanism_mat.shape', mechanism_mat.shape
00062 d3m.plot_normals(mechanism_mat, directions, color=(0,0,1.))
00063
00064 d3m.show()
00065
00066
00067
00068
00069
00070