def kitchen_cabinet_box_pr2():
#cls = 'Office Cabinet'
cls = mech = 'kitchen_cabinet_pr2'
pkl_nm = data_path + 'robot_trials/hsi_kitchen_collision_pr2/pr2_pull_2010Dec10_071602_new.pkl'
one_pkl_nm = data_path + 'robot_trials/perfect_perception/kitchen_cabinet_pr2.pkl'
# robot_trial
# 1) collision: ['ftan_list', 'ee_list', 'config_list', 'f_list', 'frad_list', 'cep_list']
# 2) perfect : ['std', 'rad', 'name', 'typ', 'vec_list', 'config', 'mean'] # vec_list gives ee_pos?
## one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/kitchen_cabinet_pr2.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 125, 128, 136)
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
x1,y1 = 14., 7.44
x2,y2 = 14.47, 8.96
x3,y3 = 14.84, 10.11
# Plot operating, applied, and expected forces
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None,
(x1, y1), (x2, y2), (x3, y3))
pp.text(x1-0.5, y1-1.5, '1', withdash=True) # pr2_box_kitchen
pp.text(x2-1.5, y2-0.7, '2', withdash=True) # pr2_box_kitchen
pp.text(x3-0.5, y3+1, '3', withdash=True) # pr2_box_kitchen
pp.ylim(-3., 16.)
pp.xlim(0., 34.)
f.subplots_adjust(bottom=.15, top=.99, right=.99)
mpu.legend(loc='lower left')
pp.savefig('collision_detection_hsi_kitchen_pr2.pdf')
pp.show()
def kitchen_cabinet_box_cody():
## cls = 'Office Cabinet'
cls = mech = 'kitchen_cabinet_cody'
pkl_nm = data_path + 'robot_trials/hsi_kitchen_collision_box/pull_trajectories_kitchen_cabinet_2010Dec10_060454_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/kitchen_cabinet_cody_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 68, 77, 104)
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 6, 10.35
x2, y2 = 6.95, 12.39
x3, y3 = 10.1314, 13.3785
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None, (x1, y1),
(x2, y2), (x3, y3))
pp.text(x1 - 0., y1 - 1.5, '1', withdash=True)
pp.text(x2 - 1., y2 - 0., '2', withdash=True)
pp.text(x3 - 0., y3 + 0.7, '3', withdash=True)
pp.ylim(-0., 16.)
pp.xlim(0., 34.)
f.subplots_adjust(bottom=.15, top=.99, right=.99)
mpu.legend(loc='lower left')
pp.savefig('collision_detection_hsi_kitchen_cody.pdf')
pp.show()
def kitchen_cabinet_box_cody():
## cls = 'Office Cabinet'
cls = mech = 'kitchen_cabinet_cody'
pkl_nm = data_path+'robot_trials/hsi_kitchen_collision_box/pull_trajectories_kitchen_cabinet_2010Dec10_060454_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/kitchen_cabinet_cody_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 68, 77, 104)
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
x1,y1 = 6, 10.35
x2, y2 = 6.95, 12.39
x3, y3 = 10.1314, 13.3785
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None,
(x1, y1), (x2, y2), (x3, y3))
pp.text(x1-0., y1-1.5, '1', withdash=True)
pp.text(x2-1., y2-0., '2', withdash=True)
pp.text(x3-0., y3+0.7, '3', withdash=True)
pp.ylim(-0., 16.)
pp.xlim(0., 34.)
f.subplots_adjust(bottom=.15, top=.99, right=.99)
mpu.legend(loc='lower left')
pp.savefig('collision_detection_hsi_kitchen_cody.pdf')
pp.show()
def fridge_box_collision():
cls = 'Fridge'
cls = mech = 'lab_fridge_cody'
pkl_nm = data_path+'robot_trials/lab_fridge_collision_box/pull_trajectories_lab_refrigerator_2010Dec10_044022_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/lab_fridge_cody_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 150, 177, 173)
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 13., 2.21
x2, y2 = 14.534, 5.75
x3, y3 = 14.2, 4.98
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None,
(x1, y1), (x2, y2), (x3, y3))
pp.text(x1-1., y1+1., '1', withdash=True)
pp.text(x2-0., y2+2., '2', withdash=True)
pp.text(x3+1.5, y3-0.5, '3', withdash=True)
pp.ylim(-0., 55.)
# pp.xlim(0., 34.)
f.subplots_adjust(bottom=.15, top=.99, right=.98)
# mpu.legend(loc='lower left')
pp.savefig('collision_detection_fridge_box.pdf')
pp.show()
def kitchen_cabinet_box_pr2():
#cls = 'Office Cabinet'
cls = mech = 'kitchen_cabinet_pr2'
pkl_nm = data_path + 'robot_trials/hsi_kitchen_collision_pr2/pr2_pull_2010Dec10_071602_new.pkl'
one_pkl_nm = data_path + 'robot_trials/perfect_perception/kitchen_cabinet_pr2.pkl'
# robot_trial
# 1) collision: ['ftan_list', 'ee_list', 'config_list', 'f_list', 'frad_list', 'cep_list']
# 2) perfect : ['std', 'rad', 'name', 'typ', 'vec_list', 'config', 'mean'] # vec_list gives ee_pos?
## one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/kitchen_cabinet_pr2.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 125, 128, 136)
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 14., 7.44
x2, y2 = 14.47, 8.96
x3, y3 = 14.84, 10.11
# Plot operating, applied, and expected forces
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None, (x1, y1),
(x2, y2), (x3, y3))
pp.text(x1 - 0.5, y1 - 1.5, '1', withdash=True) # pr2_box_kitchen
pp.text(x2 - 1.5, y2 - 0.7, '2', withdash=True) # pr2_box_kitchen
pp.text(x3 - 0.5, y3 + 1, '3', withdash=True) # pr2_box_kitchen
pp.ylim(-3., 16.)
pp.xlim(0., 34.)
f.subplots_adjust(bottom=.15, top=.99, right=.99)
mpu.legend(loc='lower left')
pp.savefig('collision_detection_hsi_kitchen_pr2.pdf')
pp.show()
def fridge_box_collision():
cls = 'Fridge'
cls = mech = 'lab_fridge_cody'
pkl_nm = data_path + 'robot_trials/lab_fridge_collision_box/pull_trajectories_lab_refrigerator_2010Dec10_044022_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/lab_fridge_cody_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 150, 177, 173)
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 13., 2.21
x2, y2 = 14.534, 5.75
x3, y3 = 14.2, 4.98
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None, (x1, y1),
(x2, y2), (x3, y3))
pp.text(x1 - 1., y1 + 1., '1', withdash=True)
pp.text(x2 - 0., y2 + 2., '2', withdash=True)
pp.text(x3 + 1.5, y3 - 0.5, '3', withdash=True)
pp.ylim(-0., 55.)
# pp.xlim(0., 34.)
f.subplots_adjust(bottom=.15, top=.99, right=.98)
# mpu.legend(loc='lower left')
pp.savefig('collision_detection_fridge_box.pdf')
pp.show()
def discrete_profile(self, plot=False):
# Get max force
for key in self.semantic.keys():
if self.force_max < max(self.semantic[key][1]):
self.force_max = max(self.semantic[key][1])
self.force_max = np.ceil(self.force_max)
# Discrete Force list
self.force_table = np.arange(0.0, self.force_max+0.000001, self.force_resol)
# Discretize it
test_dict = {}
for key in self.semantic.keys():
# Discrete Force profile
force_profile = np.zeros(self.semantic[key][1].shape)
for i,f in enumerate(self.semantic[key][1]):
force_profile[i], _ = self.find_nearest(self.force_table,f)
test_dict[key] = force_profile
# Reset transition probability matrix wrt force_max
self.reset_trans_mat()
# plot
if plot:
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
for key in self.semantic.keys():
pp.plot(self.semantic[key][1], 'r-')
pp.plot(test_dict[key],'b-')
pp.ylim(-3., 16.)
pp.xlim(0., 34.)
f.subplots_adjust(bottom=.15, top=.99, right=.99)
## mpu.legend(loc='lower left')
## pp.savefig('collision_detection_hsi_kitchen_pr2.pdf')
pp.show()
return test_dict
def discrete_profile(self, plot=False):
# Get max force
for key in self.semantic.keys():
if self.force_max < max(self.semantic[key][1]):
self.force_max = max(self.semantic[key][1])
self.force_max = np.ceil(self.force_max)
# Discrete Force list
self.force_table = np.arange(0.0, self.force_max + 0.000001,
self.force_resol)
# Discretize it
test_dict = {}
for key in self.semantic.keys():
# Discrete Force profile
force_profile = np.zeros(self.semantic[key][1].shape)
for i, f in enumerate(self.semantic[key][1]):
force_profile[i], _ = self.find_nearest(self.force_table, f)
test_dict[key] = force_profile
# Reset transition probability matrix wrt force_max
self.reset_trans_mat()
# plot
if plot:
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
for key in self.semantic.keys():
pp.plot(self.semantic[key][1], 'r-')
pp.plot(test_dict[key], 'b-')
pp.ylim(-3., 16.)
pp.xlim(0., 34.)
f.subplots_adjust(bottom=.15, top=.99, right=.99)
## mpu.legend(loc='lower left')
## pp.savefig('collision_detection_hsi_kitchen_pr2.pdf')
pp.show()
return test_dict
def fridge_chair_collision():
cls = 'Fridge'
cls = mech = 'lab_fridge_cody'
pkl_nm = data_path+'robot_trials/lab_fridge_collision_chair/pull_trajectories_lab_refrigerator_2010Dec10_042926_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/lab_fridge_cody_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 290, 295, 295)
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 25., 2.
x2, y2 = 25.14, 4.68
x3, y3 = 25.17, 5.31
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None,
(x1, y1), (x2, y2), (x3, y3))
pp.text(x1-0., y1-4., '1', withdash=True)
pp.text(x2+1., y2-2., '2', withdash=True)
pp.text(x3-1., y3+1.0, '3', withdash=True)
pp.ylim(-4., 55.)
f.subplots_adjust(bottom=.15, top=.99, right=.98)
pp.savefig('collision_detection_fridge_chair.pdf')
pp.show()
def fridge_chair_collision():
cls = 'Fridge'
cls = mech = 'lab_fridge_cody'
pkl_nm = data_path + 'robot_trials/lab_fridge_collision_chair/pull_trajectories_lab_refrigerator_2010Dec10_042926_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/lab_fridge_cody_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 290, 295, 295)
mpu.set_figure_size(10, 7.0)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 25., 2.
x2, y2 = 25.14, 4.68
x3, y3 = 25.17, 5.31
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None, (x1, y1),
(x2, y2), (x3, y3))
pp.text(x1 - 0., y1 - 4., '1', withdash=True)
pp.text(x2 + 1., y2 - 2., '2', withdash=True)
pp.text(x3 - 1., y3 + 1.0, '3', withdash=True)
pp.ylim(-4., 55.)
f.subplots_adjust(bottom=.15, top=.99, right=.98)
pp.savefig('collision_detection_fridge_chair.pdf')
pp.show()
def kitchen_cabinet_locked_cody():
cls = 'Office Cabinet'
cls = mech = 'kitchen_cabinet_cody'
pkl_nm = data_path+'robot_trials/kitchen_cabinet_locked/pull_trajectories_kitchen_cabinet_2010Dec11_233625_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/kitchen_cabinet_cody_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 14, 24, 35)
mpu.set_figure_size(10, 7)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 0.24, 7.46
x2, y2 = 0.48, 10.66
x3, y3 = 0.66, 13.52
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None,
(x1, y1), (x2, y2), (x3, y3))
pp.text(x1+1., y1-.5, '1', withdash=True)
pp.text(x2+1., y2-0., '2', withdash=True)
pp.text(x3+1., y3-0.5, '3', withdash=True)
pp.ylim(0., 16.)
pp.xlim(-0.5, 35.)
f.subplots_adjust(bottom=.16, top=.99, right=.98)
pp.savefig('locked_cody.pdf')
pp.show()
def kitchen_cabinet_locked_cody():
cls = 'Office Cabinet'
cls = mech = 'kitchen_cabinet_cody'
pkl_nm = data_path + 'robot_trials/kitchen_cabinet_locked/pull_trajectories_kitchen_cabinet_2010Dec11_233625_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/kitchen_cabinet_cody_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 14, 24, 35)
mpu.set_figure_size(10, 7)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 0.24, 7.46
x2, y2 = 0.48, 10.66
x3, y3 = 0.66, 13.52
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None, (x1, y1),
(x2, y2), (x3, y3))
pp.text(x1 + 1., y1 - .5, '1', withdash=True)
pp.text(x2 + 1., y2 - 0., '2', withdash=True)
pp.text(x3 + 1., y3 - 0.5, '3', withdash=True)
pp.ylim(0., 16.)
pp.xlim(-0.5, 35.)
f.subplots_adjust(bottom=.16, top=.99, right=.98)
pp.savefig('locked_cody.pdf')
pp.show()
def kitchen_cabinet_locked_pr2():
cls = 'Office Cabinet'
cls = mech = 'kitchen_cabinet_pr2'
#pkl_nm = 'robot_trials/kitchen_cabinet_locked/pr2_pull_2010Dec11_215502_new.pkl'
pkl_nm = data_path + 'robot_trials/kitchen_cabinet_locked/pr2_pull_2010Dec12_005340_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/kitchen_cabinet_pr2_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 49, 116, 330)
mpu.set_figure_size(10, 7)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 0.2, 7.64
x2, y2 = 0.43, 11.02
x3, y3 = 0.59, 13.42
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None,
(x1, y1), (x2, y2), (x3, y3))
pp.text(x1+1., y1-.5, '1', withdash=True)
pp.text(x2+1., y2-1., '2', withdash=True)
pp.text(x3+1., y3-0.5, '3', withdash=True)
pp.ylim(0., 16.)
pp.xlim(-0.5, 35.)
f.subplots_adjust(bottom=.16, top=.99, right=.98)
pp.savefig('locked_pr2.pdf')
pp.show()
def kitchen_cabinet_locked_pr2():
cls = 'Office Cabinet'
cls = mech = 'kitchen_cabinet_pr2'
#pkl_nm = 'robot_trials/kitchen_cabinet_locked/pr2_pull_2010Dec11_215502_new.pkl'
pkl_nm = data_path + 'robot_trials/kitchen_cabinet_locked/pr2_pull_2010Dec12_005340_new.pkl'
one_pkl_nm = pth + 'RAM_db/robot_trials/perfect_perception/kitchen_cabinet_pr2_new.pkl'
#robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, 49, 116, 330)
mpu.set_figure_size(10, 7)
f = pp.figure()
f.set_facecolor('w')
x1, y1 = 0.2, 7.64
x2, y2 = 0.43, 11.02
x3, y3 = 0.59, 13.42
robot_trial_plot(cls, mech, pkl_nm, one_pkl_nm, None, None, None, (x1, y1),
(x2, y2), (x3, y3))
pp.text(x1 + 1., y1 - .5, '1', withdash=True)
pp.text(x2 + 1., y2 - 1., '2', withdash=True)
pp.text(x3 + 1., y3 - 0.5, '3', withdash=True)
pp.ylim(0., 16.)
pp.xlim(-0.5, 35.)
f.subplots_adjust(bottom=.16, top=.99, right=.98)
pp.savefig('locked_pr2.pdf')
pp.show()
if __name__ == '__main__':
import optparse
import glob
p = optparse.OptionParser()
p.add_option('--fig_roc_human', action='store_true',
dest='fig_roc_human',
help='generate ROC like curve from the BIOROB dataset.')
opt, args = p.parse_args()
if opt.fig_roc_human:
pkl_list = glob.glob(root_path+'src/projects/modeling_forces/handheld_hook/RAM_db/*_new.pkl')
r_pkls = mar.filter_pkl_list(pkl_list, typ = 'rotary')
mech_vec_list, mech_nm_list = mar.pkls_to_mech_vec_list(r_pkls, 36)
mpu.set_figure_size(10, 7.)
pp.figure()
generate_roc_curve_no_prior(mech_vec_list, mech_nm_list)
generate_roc_curve(mech_vec_list, mech_nm_list)
f = pp.gcf()
f.subplots_adjust(bottom=.15, top=.96, right=.98, left=0.15)
pp.savefig('roc_compare.pdf')
pp.show()
else:
pkl_list = glob.glob(root_path+'src/projects/modeling_forces/handheld_hook/RAM_db/*_new.pkl') #+ glob.glob(root_path+'src/projects/modeling_forces/handheld_hook/RAM_db/robot_trials/perfect_perception/*_new.pkl') + glob.glob(root_path+'src/projects/modeling_forces/handheld_hook/RAM_db/robot_trials/simulate_perception/*_new.pkl')
## root_path = os.environ['HRLBASEPATH']
## ## pkl_list = glob.glob(root_path+'_data/usr/advait/ram_www/RAM_db_of_different_kinds/RAM_db/*_new.pkl')
else:
t_start = t_start - t_step/2
nt = nt + 1
if opt.sf:
# if we are using this file, then we need to have
# hrl_software_simulation_darpa_m3
roslib.load_manifest('hrl_software_simulation_darpa_m3')
import hrl_software_simulation_darpa_m3.gen_sim_arms as gsa
if opt.sim3:
import hrl_common_code_darpa_m3.robot_config.three_link_planar_capsule as d_robot
elif opt.sim3_with_hand:
import hrl_common_code_darpa_m3.robot_config.three_link_with_hand as d_robot
mpu.set_figure_size(6,4)
pp.figure()
kinematics = gsa.RobotSimulatorKDL(d_robot)
sssv.draw_obstacles_from_reach_problem_dict(rpd)
g_arr = np.array(g_list)
pp.scatter(-g_arr[:,1], g_arr[:,0], s=50, c='g', marker='x', lw=1, edgecolor='g')
q = [0.,0,0]
ee,_ = kinematics.FK(q)
rad = np.linalg.norm(ee)
sa = -math.radians(45)
ea = math.radians(45)
mpu.plot_circle(0., 0., rad, sa, ea, color='b', linewidth=0.5)
mpu.plot_radii(0., 0., rad, sa, ea, 2*math.pi, color='b', linewidth=0.5)
pp.xlim(-0.7, 0.7)
