plot_poses.py
Go to the documentation of this file.
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         #print 'directions.shape', directions_x.shape
00040         #print 'hand_mat.shape', hand_mat.shape
00041 
00042         #mpu.plot_yx(ut.norm(directions).A1, axis=None)
00043         #mpu.show()
00044         #mpu.plot_yx(ut.norm(hand_mat[:, 1:] - hand_mat[:, :-1]).A1, axis=None)
00045         #mpu.show()
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     #mechanism_list = []
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         #d3m.plot_points(mechanism_mat, color = (0.,0.,1.), mode='sphere')
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 #    mpu.plot_yx(pos_mat[0,:].A1, pos_mat[1,:].A1)
00067 #    mpu.show()
00068 
00069 
00070 


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