hrl_pr2_door_opening: plot.py Source File

Go to the documentation of this file.
00001 
00002 import matplotlib.pyplot as pp
00003 import numpy as np
00004 
00005 import roslib; roslib.load_manifest('hrl_pr2_door_opening')
00006 roslib.load_manifest('equilibrium_point_control')
00007 
00008 import hrl_lib.util as ut
00009 import equilibrium_point_control.arm_trajectories_ram as atr
00010 
00011 d = ut.load_pickle('pkls/ikea_cabinet_log.pkl')
00012 #d = ut.load_pickle('pkls/ikea_cabinet_2.pkl')
00013 #d = ut.load_pickle('pkls/lab_cabinet_log.pkl')
00014 
00015 typ = 'rotary'
00016 pr2_log = True
00017 
00018 d['f_list'] = d['f_list_estimate']
00019 h_config, h_ftan_estimate = atr.force_trajectory_in_hindsight(d, typ, pr2_log)
00020 pp.plot(np.degrees(h_config), h_ftan_estimate, 'ro-', mew=0, ms=0,
00021         label='estimate')
00022 
00023 if 'f_list_torques' in d:
00024     d['f_list'] = d['f_list_torques']
00025     h_config, h_ftan_torques = atr.force_trajectory_in_hindsight(d, typ,
00026                                                                  pr2_log)
00027     pp.plot(np.degrees(h_config), h_ftan_torques, 'go-', mew=0, ms=0,
00028             label='torques')
00029 
00030 d['f_list'] = d['f_list_ati']
00031 h_config, h_ftan_ati = atr.force_trajectory_in_hindsight(d, typ, pr2_log)
00032 pp.plot(np.degrees(h_config), h_ftan_ati, 'bo-', mew=0, ms=0,
00033         label='ATI')
00034 
00035 pp.legend()
00036 pp.show()
00037 
00038 
00039