def normals_cb(normals_cloud):
print 'normals_cb got called.'
d = {}
t0 = time.time()
pts = ros_pts_to_np(normals_cloud.pts)
t1 = time.time()
print 'time to go from ROS point cloud to np matrx:', t1 - t0
d['pts'] = pts
if normals_cloud.chan[0].name != 'nx':
print '################################################################################'
print 'synthetic_point_clouds.normals_cloud: DANGER DANGER normals_cloud.chan[0] is NOT nx, it is:', normals_cloud.chan[
0].name
print 'Exiting...'
print '################################################################################'
sys.exit()
normals_list = []
for i in range(3):
normals_list.append(normals_cloud.chan[i].vals)
d['normals'] = np.matrix(normals_list)
d['curvature'] = normals_cloud.chan[3].vals
print 'd[\'pts\'].shape:', d['pts'].shape
print 'd[\'normals\'].shape:', d['normals'].shape
ut.save_pickle(d, 'normals_cloud_' + ut.formatted_time() + '.pkl')
def run_canonical(self, goals, q_configs, num_canonical):
for cmd in q_configs['start']:
self.robot_state.setDesiredJointAngles(list(cmd))
self.robot_state.updateSendCmd()
time.sleep(2.)
for f_thresh in self.f_threshes:
for t_impulse in self.delta_t_s:
for i in xrange(self.num_trials):
self.robot_state.setDesiredJointAngles(list(q_configs['right_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(1.)
result = self.run_canonical_trial(i, f_thresh, t_impulse, goals, num_canonical)
for cmd in q_configs['restart']:
self.robot_state.setDesiredJointAngles(list(cmd))
self.robot_state.updateSendCmd()
time.sleep(2.)
self.robot_state.setDesiredJointAngles(list(q_configs['right_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(1.)
data2 = {}
data2['reaching_straight'] = self.reaching_right_results
ut.save_pickle(data, './combined_results_for_canonical'+str(num_canonical)+'.pkl')
def record_good_configs(use_left_arm):
import m3.toolbox as m3t
settings_arm = ar.MekaArmSettings(stiffness_list=[0., 0., 0., 0., 0.],
control_mode='torque_gc')
if use_left_arm:
firenze = ar.M3HrlRobot(connect=True, left_arm_settings=settings_arm)
arm = 'left_arm'
else:
firenze = ar.M3HrlRobot(connect=True, right_arm_settings=settings_arm)
arm = 'right_arm'
print 'hit ENTER to start the recording process'
k = m3t.get_keystroke()
firenze.power_on()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k = m3t.get_keystroke()
firenze.proxy.step()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
firenze.stop()
ut.save_pickle(good_configs_list,
ut.formatted_time() + '_good_configs_list.pkl')
def record_good_configs(use_left_arm):
import m3.toolbox as m3t
settings_arm = ar.MekaArmSettings(stiffness_list=[0.,0.,0.,0.,0.],control_mode='torque_gc')
if use_left_arm:
firenze = ar.M3HrlRobot(connect=True,left_arm_settings=settings_arm)
arm = 'left_arm'
else:
firenze = ar.M3HrlRobot(connect=True,right_arm_settings=settings_arm)
arm = 'right_arm'
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
firenze.power_on()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
firenze.proxy.step()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
firenze.stop()
ut.save_pickle(good_configs_list,ut.formatted_time()+'_good_configs_list.pkl')
def normals_cb(normals_cloud):
print "normals_cb got called."
d = {}
t0 = time.time()
pts = ros_pts_to_np(normals_cloud.pts)
t1 = time.time()
print "time to go from ROS point cloud to np matrx:", t1 - t0
d["pts"] = pts
if normals_cloud.chan[0].name != "nx":
print "################################################################################"
print "synthetic_point_clouds.normals_cloud: DANGER DANGER normals_cloud.chan[0] is NOT nx, it is:", normals_cloud.chan[
0
].name
print "Exiting..."
print "################################################################################"
sys.exit()
normals_list = []
for i in range(3):
normals_list.append(normals_cloud.chan[i].vals)
d["normals"] = np.matrix(normals_list)
d["curvature"] = normals_cloud.chan[3].vals
print "d['pts'].shape:", d["pts"].shape
print "d['normals'].shape:", d["normals"].shape
ut.save_pickle(d, "normals_cloud_" + ut.formatted_time() + ".pkl")
def close_log_file(self, trial_name):
# Finish data collection
if self.ft: self.ft.cancel()
if self.audio: self.audio.cancel()
if self.kinematics: self.kinematics.cancel()
d = {}
d['init_time'] = self.init_time
if self.ft:
## dict['force'] = self.ft.force_data
## dict['torque'] = self.ft.torque_data
d['ft_force_raw'] = self.ft.force_raw_data
d['ft_torque_raw'] = self.ft.torque_raw_data
d['ft_time'] = self.ft.time_data
if self.audio:
d['audio_data'] = self.audio.audio_data
d['audio_amp'] = self.audio.audio_amp
d['audio_freq'] = self.audio.audio_freq
d['audio_chunk'] = self.audio.CHUNK
d['audio_time'] = self.audio.time_data
if self.kinematics:
d['kinematics_time'] = self.kinematics.time_data
d['kinematics_joint'] = self.kinematics.joint_data
## if trial_name is not None: self.trial_name = trial_name
## else:
flag = raw_input("Enter trial's name (e.g. 1:success, 2:failure_reason, 3: exit): ")
#flag = "1"
if flag == "1": self.trial_name = 'success'
elif flag == "2": self.trial_name = trial_name
elif flag == "3": sys.exit()
else: self.trial_name = flag
self.file_name = self.sub_name+'_'+self.task_name+'_'+self.actor+'_'+self.trial_name
pkl_list = glob.glob('*.pkl')
max_num = 0
for pkl in pkl_list:
if pkl.find(self.file_name)>=0:
num = int(pkl.split('_')[-1].split('.')[0])
if max_num < num:
max_num = num
max_num = int(max_num)+1
self.pkl = self.file_name+'_'+str(max_num)+'.pkl'
print "File name: ", self.pkl
ut.save_pickle(d, self.pkl)
## self.tool_tracker_log_file.close()
## self.tooltip_log_file.close()
## self.head_tracker_log_file.close()
## self.gen_log_file.close()
print 'Closing.. log files have saved..'
def delete_autobed_configuration(self, req):
""" Is the callback from the GUI Interaction Service. This service is
called when a user wants to delete a saved position. This function will
update the autobed_config_data.yaml file by removing the data
corresponding to the string specified. If successful, it will return a
boolean value"""
try:
#Get Autobed pickle file from the params directory into a dictionary
current_autobed_config_data = load_pickle(self.autobed_config_file)
except:
#return zero
return update_bed_configResponse(False)
#Delete present Autobed positions and angles from the dictionary
try:
current_autobed_config_data.pop(req.config)
except KeyError:
update_bed_configResponse(False)
print "Autobed Configuration Database is already empty"
try:
#Update param file
save_pickle(current_autobed_config_data, self.autobed_config_file)
#Publish list of keys to the abdout1 topic
self.abdout1.publish(current_autobed_config_data.keys())
#Return success if param file correctly updated
return update_bed_configResponse(True)
except:
return update_bed_configResponse(False)
def test_sinusoid(z,A0,A,freq):
# rospack = rospkg.RosPack()
# pkg_path = rospack.get_path('efri')
# res_dict = np.array([1,1,1])
# save_pickle(res_dict,'/home/ari/sinusoid.pkl')
# save_pickle(res_dict, ''.join([pkg_path, 'sinusoid_data.pkl']))
w = 2*math.pi*freq
period = 1./freq
t_end = 10*period
if t_end>10:
t_end = 5*period
z.move_position(A0+A,0.1,0.05,blocking=True)
time.sleep(2.0)
#z.move_position(A0)
z.use_velocity_mode()
deck_t = deque()
deck_x = deque()
t0 = rospy.Time.now()
# t0 = time.time()
while True:
tnow = rospy.Time.now()
# tnow = time.time()
t_diff = rospy.Time.now().to_sec()-t0.to_sec()
v = -w*A*math.sin(w*t_diff)
a = -w*w*A*np.cos(w*t_diff)
z.set_velocity(v)
z.set_acceleration(a)
z.go()
time.sleep(0.025)
new_pos = z.get_position_meters()
deck_x.append(new_pos)
t_diff = rospy.Time.now().to_sec()-t0.to_sec()
deck_t.append(t_diff)
if t_diff >= t_end:
break
z.estop()
time.sleep(0.1)
z.use_position_mode()
time.sleep(0.1)
z.move_position(A0,0.1,0.05,blocking=True)
res_dict = {}
res_dict['x'] = deck_x
res_dict['t'] = deck_t
res_dict['A0'] = A0
res_dict['A'] = A
res_dict['freq'] = freq
res_dict['t_end'] = t_end
ut.save_pickle(res_dict,'/home/ari/sinusoid.pkl')
def test_sinusoid(z, A0, A, freq):
# rospack = rospkg.RosPack()
# pkg_path = rospack.get_path('efri')
# res_dict = np.array([1,1,1])
# save_pickle(res_dict,'/home/ari/sinusoid.pkl')
# save_pickle(res_dict, ''.join([pkg_path, 'sinusoid_data.pkl']))
w = 2 * math.pi * freq
period = 1. / freq
t_end = 10 * period
if t_end > 10:
t_end = 5 * period
z.move_position(A0 + A, 0.1, 0.05, blocking=True)
time.sleep(2.0)
#z.move_position(A0)
z.use_velocity_mode()
deck_t = deque()
deck_x = deque()
t0 = rospy.Time.now()
# t0 = time.time()
while True:
tnow = rospy.Time.now()
# tnow = time.time()
t_diff = rospy.Time.now().to_sec() - t0.to_sec()
v = -w * A * math.sin(w * t_diff)
a = -w * w * A * np.cos(w * t_diff)
z.set_velocity(v)
z.set_acceleration(a)
z.go()
time.sleep(0.025)
new_pos = z.get_position_meters()
deck_x.append(new_pos)
t_diff = rospy.Time.now().to_sec() - t0.to_sec()
deck_t.append(t_diff)
if t_diff >= t_end:
break
z.estop()
time.sleep(0.1)
z.use_position_mode()
time.sleep(0.1)
z.move_position(A0, 0.1, 0.05, blocking=True)
res_dict = {}
res_dict['x'] = deck_x
res_dict['t'] = deck_t
res_dict['A0'] = A0
res_dict['A'] = A
res_dict['freq'] = freq
res_dict['t_end'] = t_end
ut.save_pickle(res_dict, '/home/ari/sinusoid.pkl')
def run_foliage_reach(self, goals, q_configs, num_reach):
if self.first_reach == True:
self.first_reach = False
for cmd in q_configs['start']:
self.robot_state.setDesiredJointAngles(list(cmd))
self.robot_state.updateSendCmd()
time.sleep(2.)
for f_thresh in self.f_threshes:
for t_impulse in self.delta_t_s:
for i in xrange(self.num_trials):
self.robot_state.setDesiredJointAngles(list(q_configs['trial_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(1.)
result = self.run_foliage_trial(i, f_thresh, t_impulse, goals, num_reach)
offset = 0.20 - goals[2]
goals[2] = goals[2]+offset
counter = (str(i)+'_up').zfill(6)
reset_result = self.run_foliage_trial(counter, f_thresh, t_impulse, goals, num_reach)
reset_result = self.run_foliage_trial(i, f_thresh, t_impulse, self.goal_reset, num_reach, record = False)
if result != 'success':
raw_input('Help me a bit please ..')
for cmd in q_configs['restart']:
self.robot_state.setDesiredJointAngles(list(cmd))
self.robot_state.updateSendCmd()
time.sleep(2.)
self.robot_state.setDesiredJointAngles(list(q_configs['trial_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(1.)
data2 = {}
data2['reaching_straight'] = self.reaching_right_results
ut.save_pickle(data, './combined_results_for_foliage.pkl')
def save_data(self, name, metadata=True, angle=None):
dict = {
'laserscans': self.laserscans,
'l1': self.config.thok_l1,
'l2': self.config.thok_l2,
'image_angle': angle
}
prefix = self.config.path + '/data/' + name
print "Saving: " + prefix + '_laserscans.pkl'
ut.save_pickle(dict, prefix + '_laserscans.pkl')
print "Saving: " + prefix + '_image.png'
highgui.cvSaveImage(prefix + '_image.png', self.img)
if metadata:
# save metadata to database:
database = scans_database.scans_database()
database.load(self.config.path, 'database.pkl')
dataset = scan_dataset.scan_dataset()
dataset.id = name
dataset.scan_filename = 'data/' + name + '_laserscans.pkl'
dataset.image_filename = 'data/' + name + '_image.png'
database.add_dataset(dataset)
database.save()
return name
def record_initial(firenze):
equilibrium_pos_list = []
for i in range(50):
equilibrium_pos_list.append(firenze.end_effector_pos('right_arm'))
eq_pos = np.column_stack(equilibrium_pos_list).mean(1)
ut.save_pickle(eq_pos, 'eq_pos_' + ut.formatted_time() + '.pkl')
firenze.bias_wrist_ft('right_arm')
def record_initial(firenze):
equilibrium_pos_list = []
for i in range(50):
equilibrium_pos_list.append(firenze.end_effector_pos('right_arm'))
eq_pos = np.column_stack(equilibrium_pos_list).mean(1)
ut.save_pickle(eq_pos,'eq_pos_'+ut.formatted_time()+'.pkl')
firenze.bias_wrist_ft('right_arm')
def set_position_data_from_saved_movement_profile(self, subject, input_classes, output_classes):
print 'Now editing files to insert position data.'
position_profile = None
for vel in [0.1, 0.15]:
if vel == 0.1:
print ''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl'])
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']))
print 'Position profile loaded!'
elif vel == 0.15:
position_profile = load_pickle(''.join([self.data_path, '/data/position_profiles/position_combined_0_15mps.pkl']))
print ''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl'])
print 'Position profile loaded!'
else:
print 'There is no saved position profile for this velocity! Something has gone wrong!'
return None
for class_num in xrange(len(input_classes)):
i = 0
while os.path.isfile(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl'])):
ft_threshold_was_exceeded = False
print ''.join([self.data_path, '/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl'])
# current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.pkg_path, '/data/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.log']))])
current_data = load_pickle(''.join([self.data_path, '/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl']))
# if np.max(current_data[:, 2]) >= 10. or np.max(current_data[:, 3]) >= 10. \
# or np.max(current_data[:, 4]) >= 10.:
# ft_threshold_was_exceeded = True
for j in current_data:
j[2] = -j[2]
j[3] = -j[3]
j[4] = -j[4]
if j[0] < 0.5:
j[1] = 0
else:
if np.max(np.abs(j[2:])) > 10. and not ft_threshold_was_exceeded:
time_of_stop = j[0]
ft_threshold_was_exceeded = True
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
position_of_stop = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
if ft_threshold_was_exceeded:
j[1] = position_of_stop
else:
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
new_position = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
save_number = 0
while os.path.isfile(''.join([self.data_path, '/', subject, '/formatted/', str(vel),'mps/', output_classes[class_num], '/force_profile_', str(save_number), '.pkl'])):
save_number += 1
print 'Saving with number', save_number
save_pickle(current_data, ''.join([self.data_path, '/', subject, '/formatted/', str(vel),'mps/', output_classes[class_num], '/force_profile_', str(save_number), '.pkl']))
i += 1
print 'Done editing files!'
def save_mouse_traj(self, pkl_file):
# save pkl
data = {}
data['all_trajs'] = self.all_trajs
data['start'] = self.aStart
data['goal'] = self.aGoal
ut.save_pickle(data,pkl_file)
print "Raw Data Pkl Saved"
def compress_pkl(surf_path_complete):
features_list = ut.load_pickle(surf_path_complete)
rospy.loginfo('making matrix')
fmat = features_mat(features_list)
rospy.loginfo('compressing')
reduced_features = features_mat_compress(fmat, 1000)
small_pickle_fname = gen_pkl_name(path_complete, '.surf_sm_pkl')
ut.save_pickle(reduced_features, small_pickle_fname)
rospy.loginfo('saved to %s' % small_pickle_fname)
def save(self):
dict = {'datasets': self.datasets,'version': 0.1}
#for now: make a backup first:
database_filename = self.path+'/'+self.filename
backup_filename = self.path+'/'+self.filename+'_backup_'+ut.formatted_time()
print 'Backing up old database to ' + backup_filename
shutil.copy(database_filename, backup_filename)
print "Saving: "+database_filename
ut.save_pickle(dict,database_filename)
def create_dict(fname):
firenze = cak.CodyArmKinematics('r')
good_configs_list = ut.load_pickle(fname)
cartesian_points_list = []
for gc in good_configs_list:
cartesian_points_list.append(firenze.FK(gc).A1.tolist())
m = np.matrix(cartesian_points_list).T
print 'm.shape:', m.shape
dic = {'cart_pts_mat':m, 'good_configs_list':good_configs_list}
ut.save_pickle(dic,ut.formatted_time()+'_goodconf_dict.pkl')
def stop_recording_data(self, num):
self.recording = False
# self.arm_file.close()
self.sleeve_file.close()
# self.position_file.close()
save_pickle(
self.array_to_save, ''.join([
self.data_path, '/', self.subject, '/',
str(self.test_vel), 'mps/', self.height, '/ft_sleeve_',
str(num), '.pkl'
]))
def setCalibration(self, filename='bowl_frame.pkl'):
self.bowl_calib = True
print "------------------------------------"
print "Calibration complete! - bowl_cen offset"
print "------------------------------------"
print "P: ", self.bowl_cen_frame_off.p
print "M: ", self.bowl_cen_frame_off.M
print "------------------------------------"
d = {}
d['frame'] = self.bowl_cen_frame_off
ut.save_pickle(d,filename)
def compress_pkl(surf_path_complete):
features_list = ut.load_pickle(surf_path_complete)
rospy.loginfo('making matrix')
fmat = features_mat(features_list)
rospy.loginfo('compressing')
reduced_features = features_mat_compress(fmat, 1000)
small_pickle_fname = gen_pkl_name(path_complete, '.surf_sm_pkl')
ut.save_pickle(reduced_features, small_pickle_fname)
rospy.loginfo('saved to %s' % small_pickle_fname)
def setCalibration(self, filename='bowl_frame.pkl'):
self.bowl_calib = True
print "------------------------------------"
print "Calibration complete! - bowl_cen offset"
print "------------------------------------"
print "P: ", self.bowl_cen_frame_off.p
print "M: ", self.bowl_cen_frame_off.M
print "------------------------------------"
d = {}
d['frame'] = self.bowl_cen_frame_off
ut.save_pickle(d,filename)
def create_dict(fname):
firenze = ar.M3HrlRobot(connect=False)
good_configs_list = ut.load_pickle(fname)
cartesian_points_list = []
for gc in good_configs_list:
cartesian_points_list.append(firenze.FK('right_arm',gc).A1.tolist())
m = np.matrix(cartesian_points_list).T
print 'm.shape:', m.shape
dict = {'cart_pts_mat':m, 'good_configs_list':good_configs_list}
ut.save_pickle(dict,ut.formatted_time()+'_goodconf_dict.pkl')
def create_dict(fname):
firenze = ar.M3HrlRobot(connect=False)
good_configs_list = ut.load_pickle(fname)
cartesian_points_list = []
for gc in good_configs_list:
cartesian_points_list.append(firenze.FK('right_arm', gc).A1.tolist())
m = np.matrix(cartesian_points_list).T
print 'm.shape:', m.shape
dict = {'cart_pts_mat': m, 'good_configs_list': good_configs_list}
ut.save_pickle(dict, ut.formatted_time() + '_goodconf_dict.pkl')
def pull(self, arm, force_threshold, cep_vel, mechanism_type=''):
self.mechanism_type = mechanism_type
self.stopping_string = ''
self.eq_pt_not_moving_counter = 0
self.init_log()
self.init_tangent_vector = None
self.open_ang_exceed_count = 0.
self.eq_force_threshold = force_threshold
self.ftan_threshold = 2.
self.hooked_location_moved = False # flag to indicate when the hooking location started moving.
self.prev_force_mag = np.linalg.norm(self.robot.get_wrist_force(arm))
self.slip_count = 0
self.started_pulling_on_handle = False
self.started_pulling_on_handle_count = 0
ee_pos, _ = self.robot.get_ee_jtt(arm)
self.cx_start = ee_pos[0, 0]
self.rad = 10.0
self.cy_start = ee_pos[1, 0] - self.rad
self.cz_start = ee_pos[2, 0]
cep, _ = self.robot.get_cep_jtt(arm)
arg_list = [arm, cep, cep_vel]
result, _ = self.epc_motion(
self.cep_gen_control_radial_force,
0.1,
arm,
arg_list,
self.log_state,
#0.01, arm, arg_list,
control_function=self.robot.set_cep_jtt)
print 'EPC motion result:', result
print 'Original force threshold:', force_threshold
print 'Adapted force threshold:', self.eq_force_threshold
print 'Adapted ftan threshold:', self.ftan_threshold
d = {
'f_list': self.f_list,
'ee_list': self.ee_list,
'cep_list': self.cep_list,
'ftan_list': self.ft.tangential_force,
'config_list': self.ft.configuration,
'frad_list': self.ft.radial_force,
'f_list_ati': self.f_list_ati,
'f_list_estimate': self.f_list_estimate,
'f_list_torques': self.f_list_torques
}
ut.save_pickle(d, 'pr2_pull_' + ut.formatted_time() + '.pkl')
def setCalibration(self, filename='mouth_frame.pkl'):
self.head_calib = True
print "------------------------------------"
print "Calibration complete! - mouth offset"
print "------------------------------------"
print "P: ", self.mouth_frame_off.p
print "M: ", self.mouth_frame_off.M
print "------------------------------------"
d = {}
d['frame'] = self.mouth_frame_off
ut.save_pickle(d,filename)
def create_workspace_dict():
dd = {}
ha_list = [math.radians(d) for d in [0.,90.,-90.]]
for ha in ha_list:
d = {}
for z in np.arange(-0.1,-0.55,-0.01):
print 'z:',z
pts2d = compute_workspace(z,hook_angle=ha)
d[z] = pts2d
dd[ha] = {}
dd[ha]['pts'] = d
ut.save_pickle(dd,'workspace_dict_'+ut.formatted_time()+'.pkl')
def run_first_impact(self, goals, q_configs):
for cmd in q_configs['start']:
self.robot_state.setDesiredJointAngles(list(cmd))
self.robot_state.updateSendCmd()
time.sleep(2.)
for f_thresh in self.f_threshes:
for t_impulse in self.delta_t_s:
for i in xrange(self.num_trials):
self.robot_state.setDesiredJointAngles(list(q_configs['left_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(1.)
side = 'left'
result = self.run_trial(i, side, f_thresh, t_impulse, goals[side])
if result == 'success':
self.robot_state.setDesiredJointAngles(list(q_configs['right_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(2.)
else:
for cmd in q_configs['left_to_right_restart']:
self.robot_state.setDesiredJointAngles(list(cmd))
self.robot_state.updateSendCmd()
time.sleep(2.)
self.robot_state.setDesiredJointAngles(list(q_configs['right_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(1.)
side = 'right'
result = self.run_trial(i, side, f_thresh, t_impulse, goals[side])
if result == 'success':
self.robot_state.setDesiredJointAngles(list(q_configs['left_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(2.)
else:
for cmd in q_configs['right_to_left_restart']:
self.robot_state.setDesiredJointAngles(list(cmd))
self.robot_state.updateSendCmd()
time.sleep(2.)
self.robot_state.setDesiredJointAngles(list(q_configs['left_start'][0]))
self.robot_state.updateSendCmd()
time.sleep(1.)
data2 = {}
data2['reaching_left'] = self.reaching_left_results
data2['reaching_right'] = self.reaching_right_results
ut.save_pickle(data, './combined_results_for_first_impact.pkl')
def save_trials_thermistor():
counter = 0
for i in range(30):
data_heat = run_exp(P_Gain=8e-4,
I_Gain=0,
D_Gain=8e-3,
set_temp=35,
duration=60.,
Material='_')
data_cool = run_exp(cooling=True, duration=60., Material='_')
ut.save_pickle(data_heat, 'thermistor/%d.pkl' % counter)
counter += 1
ut.save_pickle(data_cool, 'thermistor/%d.pkl' % counter)
counter += 1
def __init__(self, file_number, subject_number, video=False):
self.video = video
self.count = 0
self.bridge = CvBridge()
self.lock = RLock()
self.head_pose = []
self.depth_img = []
self.camera_depth_info = None
self.camera_rgb_info = None
self.rgb_img = []
self.file_number = file_number
self.subject_number = subject_number
self.head_pose_sub = rospy.Subscriber('/haptic_mpc/head_pose',
PoseStamped, self.head_pose_cb)
self.listener = tf.TransformListener()
rospack = rospkg.RosPack()
self.pkg_path = rospack.get_path('hrl_head_tracking')
# self.pkg_path = '/home/ari/git/gt-ros-pkg.hrl_assistive/hrl_head_tracking/'
# depth_img_path = '/head_mount_kinect/depth/points'
# self.depth_img_sub = rospy.Subscriber(depth_img_path, PointCloud2, self.depth_img_cb)
# depth_img_path = '/head_mount_kinect/depth_registered/image_rect'
# self.depth_img_sub = rospy.Subscriber(depth_img_path, Image, self.depth_img_cb)
# depth_img_path = '/head_mount_kinect/depth_registered/image_raw'
# self.depth_img_sub = rospy.Subscriber(depth_img_path, Image, self.depth_img_cb)
# depth_img_path = '/head_mount_kinect/depth/image/compressed'
# self.depth_img_sub = rospy.Subscriber(depth_img_path, CompressedImage, self.depth_img_cb)
# rgb_img_path = '/head_mount_kinect/rgb/image_color/compressed'
# self.rgb_img_sub = rospy.Subscriber(rgb_img_path, CompressedImage, self.rgb_img_cb)
rgb_imgpath = '/head_mount_kinect/rgb_rect/image'
self.rgb_img_sub = rospy.Subscriber(rgb_imgpath, Image,
self.rgb_img_cb)
rgb_infopath = '/head_mount_kinect/rgb_rect/camera_info'
self.camera_rgb_info_sub = rospy.Subscriber(rgb_infopath, CameraInfo,
self.camera_rgb_info_cb)
# depth_infopath = '/head_mount_kinect/depth/camera_info'
# self.camera_depth_info_sub = rospy.Subscriber(depth_infopath, CameraInfo, self.camera_depth_info_cb)
rospy.sleep(3)
# if self.camera_depth_info is not None:
# save_pickle(self.camera_depth_info, ''.join([self.pkg_path, '/data/', 'camera_depth_info.pkl']))
# else:
# print 'Depth camera info was not ready yet. Should probably try to get it later'
if self.camera_rgb_info is not None:
save_pickle(
self.camera_rgb_info,
''.join([self.pkg_path, '/data/', 'camera_rgb_info.pkl']))
else:
print 'RGB camera info was not ready yet. Should probably try to get it later'
print 'Ready to record Ground Truth head pose!'
def create_workspace_boundary(pkl_name):
dd = ut.load_pickle(pkl_name)
for ha in dd.keys():
pts_dict = dd[ha]['pts']
bndry_dict = {}
for z in pts_dict.keys():
print 'z:',z
wrkspc = pts_dict[z]
if wrkspc.shape[1] < 100:
pts_dict.pop(z)
continue
bndry = smc.compute_boundary(wrkspc)
bndry_dict[z] = bndry
dd[ha]['bndry'] = bndry_dict
ut.save_pickle(dd, pkl_name)
def convert_to_ram_db(pkls, name):
if pkls == []:
return
bin_size = math.radians(1.)
mean, std, c, arr = compute_mean_std(pkls, bin_size)
d = {}
d['std'] = std
d['mean'] = mean
d['rad'] = -3.141592
d['name'] = name
d['config'] = c
d['vec_list'] = arr.tolist()
d['typ'] = 'rotary'
ut.save_pickle(d, name + '.pkl')
def convert_to_ram_db(pkls, name):
if pkls == []:
return
bin_size = math.radians(1.)
mean, std, c, arr = compute_mean_std(pkls, bin_size)
d = {}
d['std'] = std
d['mean'] = mean
d['rad'] = -3.141592
d['name'] = name
d['config'] = c
d['vec_list'] = arr.tolist()
d['typ'] = 'rotary'
ut.save_pickle(d, name+'.pkl')
def record_joint_displacements():
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
pos_list = []
force_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
firenze.proxy.step()
pos_list.append(firenze.end_effector_pos('right_arm'))
force_list.append(firenze.get_wrist_force('right_arm', base_frame=True))
ut.save_pickle({'pos_list':pos_list,'force_list':force_list},'stiffness_'+ut.formatted_time()+'.pkl')
firenze.stop()
def get_data(pkl_file, mech_class='Office Cabinet', verbose=False, renew=False):
######################################################
# Get Training Data
if os.path.isfile(pkl_file) and renew==False:
print "Saved pickle found"
data = ut.load_pickle(pkl_file)
data_vecs = data['data_vecs']
data_mech = data['data_mech']
data_chunks = data['data_chunks']
else:
print "No saved pickle found"
data_vecs, data_mech, data_chunks = get_all_blocked_detection()
data = {}
data['data_vecs'] = data_vecs
data['data_mech'] = data_mech
data['data_chunks'] = data_chunks
ut.save_pickle(data,pkl_file)
## from collections import OrderedDict
## print list(OrderedDict.fromkeys(data_mech)), len(list(OrderedDict.fromkeys(data_mech)))
## print list(OrderedDict.fromkeys(data_chunks)), len(list(OrderedDict.fromkeys(data_chunks)))
## print len(data_mech), data_vecs.shape
## sys.exit()
# Filtering
idxs = np.where([mech_class in i for i in data_mech])[0].tolist()
print "Load ", mech_class
## print data_mech
## print data_vecs.shape, np.array(data_mech).shape, np.array(data_chunks).shape
data_vecs = data_vecs[:,idxs]
data_mech = [data_mech[i] for i in idxs]
data_chunks = [data_chunks[i] for i in idxs]
## X data
data_vecs = np.array([data_vecs.T]) # category x number_of_data x profile_length
data_vecs[0] = approx_missing_value(data_vecs[0])
## ## time step data
## m, n = data_vecs[0].shape
## aXData = np.array([np.arange(0.0,float(n)-0.0001,1.0).tolist()] * m)
if verbose==True:
print data_vecs.shape, np.array(data_mech).shape, np.array(data_chunks).shape
return data_vecs, data_mech, data_chunks
def pull(self, arm, force_threshold, cep_vel, mechanism_type=''):
self.mechanism_type = mechanism_type
self.stopping_string = ''
self.eq_pt_not_moving_counter = 0
self.init_log()
self.init_tangent_vector = None
self.open_ang_exceed_count = 0.
self.eq_force_threshold = force_threshold
self.ftan_threshold = 2.
self.hooked_location_moved = False # flag to indicate when the hooking location started moving.
self.prev_force_mag = np.linalg.norm(self.robot.get_wrist_force(arm))
self.slip_count = 0
self.started_pulling_on_handle = False
self.started_pulling_on_handle_count = 0
ee_pos, _ = self.robot.get_ee_jtt(arm)
self.cx_start = ee_pos[0,0]
self.rad = 10.0
self.cy_start = ee_pos[1,0]-self.rad
self.cz_start = ee_pos[2,0]
cep, _ = self.robot.get_cep_jtt(arm)
arg_list = [arm, cep, cep_vel]
result, _ = self.epc_motion(self.cep_gen_control_radial_force,
0.1, arm, arg_list, self.log_state,
#0.01, arm, arg_list,
control_function = self.robot.set_cep_jtt)
print 'EPC motion result:', result
print 'Original force threshold:', force_threshold
print 'Adapted force threshold:', self.eq_force_threshold
print 'Adapted ftan threshold:', self.ftan_threshold
d = {
'f_list': self.f_list, 'ee_list': self.ee_list,
'cep_list': self.cep_list, 'ftan_list': self.ft.tangential_force,
'config_list': self.ft.configuration, 'frad_list': self.ft.radial_force,
'f_list_ati': self.f_list_ati,
'f_list_estimate': self.f_list_estimate,
'f_list_torques': self.f_list_torques
}
ut.save_pickle(d,'pr2_pull_'+ut.formatted_time()+'.pkl')
def param_estimation(self, tuned_parameters, nFold, save_file=None):
# nFold: integer and less than the total number of samples.
nSample = len(self.aXData)
# Variable check
if nFold > nSample:
print "Wrong nVfold number"
sys.exit()
# Split the dataset in two equal parts
X_train = self.aXData
print("Tuning hyper-parameters for %s :", X_train.shape)
print()
clf = GridSearchCV(self, tuned_parameters, cv=nFold, n_jobs=8)
clf.fit(X_train) # [n_samples, n_features]
print("Best parameters set found on development set:")
print()
print(clf.best_estimator_)
print()
print("Grid scores on development set:")
print()
params_list = []
mean_list = []
std_list = []
for params, mean_score, scores in clf.grid_scores_:
print("%0.3f (+/-%0.03f) for %r"
% (mean_score, scores.std() / 2, params))
params_list.append(params)
mean_list.append(mean_score)
std_list.append(scores.std())
print()
# Save data
data = {}
data['mean'] = mean_list
data['std'] = std_list
data['params'] = params_list
if save_file is None:
save_file='tune_data.pkl'
ut.save_pickle(data, save_file)
def param_estimation(self, tuned_parameters, nFold, save_file=None):
# nFold: integer and less than the total number of samples.
nSample = len(self.aXData)
# Variable check
if nFold > nSample:
print "Wrong nVfold number"
sys.exit()
# Split the dataset in two equal parts
X_train = self.aXData
print("Tuning hyper-parameters for %s :", X_train.shape)
print()
clf = GridSearchCV(self, tuned_parameters, cv=nFold, n_jobs=8)
clf.fit(X_train) # [n_samples, n_features]
print("Best parameters set found on development set:")
print()
print(clf.best_estimator_)
print()
print("Grid scores on development set:")
print()
params_list = []
mean_list = []
std_list = []
for params, mean_score, scores in clf.grid_scores_:
print("%0.3f (+/-%0.03f) for %r" %
(mean_score, scores.std() / 2, params))
params_list.append(params)
mean_list.append(mean_score)
std_list.append(scores.std())
print()
# Save data
data = {}
data['mean'] = mean_list
data['std'] = std_list
data['params'] = params_list
if save_file is None:
save_file = 'tune_data.pkl'
ut.save_pickle(data, save_file)
def generate_score(self, viz_rviz=False, visualize=False, plot=False):
cachedir = mkdtemp()
# memory = Memory(cachedir=cachedir, verbose=0)
memory = Memory(cachedir=cachedir, mmap_mode='r')
self.num_base_locations = 1
mytargets = 'all_goals'
mytask = self.task
myReferenceNames = self.reference_options
myGoals = copy.copy(self.goal_unique) # [self.data_start:self.data_finish]
print 'There are ', len(myGoals), ' goals being sent to score generator.'
selector = ScoreGenerator(visualize=visualize, targets=mytargets, reference_names=myReferenceNames,
goals=myGoals, model=self.model)#, tf_listener=self.tf_listener)
if viz_rviz:
selector.show_rviz()
score_sheet = selector.handle_score_generation(plot=plot)
# default_is_zero = False
# if not score_sheet[0., 0.]:
# print 'There are no good results in the score sheet'
# print score_sheet[0]
# else:
# print 'The score sheet top item: '
# print '([heady, distance, x, y, theta, z, bed_z, bed_head_rest_theta, score])'
# print 'See the created pkl file for the entire set of data.'
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('hrl_base_selection')
if self.task == 'shaving' or True:
# print 'Using the alternative streaming method for saving data because it is a big data set.'
# filename = ''.join([pkg_path, '/data/', self.task, '_', self.model, '_subj_', str(self.sub_num),
# '_score_data.pkl'])
# filename = ''.join([pkg_path, '/data/', self.task, '_', self.model, '_cma_real_expanded_',
filename = ''.join([pkg_path, '/data/', self.task, '_', self.model, '_cma_real_expanded_no_bed_movement',
# '_real_expanded_',
'_score_data.pkl'])
save_pickle(score_sheet, filename)
# filename = ''.join([pkg_path, '/data/', self.task, '_', self.model, '_subj_', str(self.sub_num),
# '_score_data'])
# joblib.dump(score_sheet, filename)
else:
save_pickle(score_sheet, ''.join([pkg_path, '/data/', self.task, '_', self.model, '_quick_score_data.pkl']))
print 'I saved the data successfully!'
return score_sheet
def test_IK(arm, rot_mat):
''' try out the IK at a number of different cartesian
points in the workspace, with the given rotation
matrix for the end effector.
'''
print 'press ENTER to start.'
k=m3t.get_keystroke()
while k=='\r':
p = firenze.end_effector_pos(arm)
firenze.go_cartesian(arm,p,rot_mat,speed=0.1)
firenze.step()
print 'press ENTER to save joint angles.'
k=m3t.get_keystroke()
if k == '\r':
firenze.step()
q = firenze.get_joint_angles(arm)
ut.save_pickle(q,'arm_config_'+ut.formatted_time()+'.pkl')
print 'press ENTER for next IK test. something else to exit.'
k=m3t.get_keystroke()
def close_log_file(self):
self.enable_log_thread = False
flag = raw_input('Enter trial\'s status (e.g. 1:success, 2:failure, 3: skip): ')
if flag == '1': status = 'success'
elif flag == '2': status = 'failure'
elif flag == '3': status = 'skip'
else: status = flag
if status == 'success' or status == 'failure':
if status == 'failure':
failure_class = raw_input('Enter failure reason if there is: ')
if not os.path.exists(self.folderName): os.makedirs(self.folderName)
# get next file id
if status == 'success':
fileList = util.getSubjectFileList(self.record_root_path, [self.subject], self.task)[0]
else:
fileList = util.getSubjectFileList(self.record_root_path, [self.subject], self.task)[1]
test_id = -1
if len(fileList)>0:
for f in fileList:
if test_id < int((os.path.split(f)[1]).split('_')[1]):
test_id = int((os.path.split(f)[1]).split('_')[1])
if status == 'failure':
fileName = os.path.join(self.folderName, 'iteration_%d_%s_%s.pkl' % (test_id+1, status, failure_class))
else:
fileName = os.path.join(self.folderName, 'iteration_%d_%s.pkl' % (test_id+1, status))
print 'Saving to', fileName
ut.save_pickle(self.data, fileName)
# Reinitialize all sensors
## if self.audio is not None: self.audio = kinect_audio()
## if self.kinematics is not None: self.kinematics = robot_kinematics() #.initVars() #
## if self.ft is not None: self.ft = tool_ft()
## if self.vision is not None: self.vision = artag_vision()
## if self.pps_skin is not None: self.pps_skin = pps_skin()
gc.collect()
rospy.sleep(1.0)
def split_open_close(rad, tan, ang, typ, mech_radius, time_list,
moment_axis, moment_tip):
ang = np.array(ang)
incr = ang[1:] - ang[:-1]
n_pts = ang.shape[0] - 2 #ignoring first and last readings.
rad_l, tan_l, ang_l = [], [], []
for i in range(n_pts):
if typ == 'rotary':
sgn = incr[i] * incr[i+1]
mag = abs(incr[i] - incr[i+1])
if sgn < 0 and mag > math.radians(10):
continue
rad_l.append(rad[i+1])
tan_l.append(tan[i+1])
ang_l.append(ang[i+1])
else:
# no cleanup for prismatic joints, for now
rad_l.append(rad[i+1])
tan_l.append(tan[i+1])
ang_l.append(ang[i+1])
rad, tan, ang = rad_l, tan_l, ang_l
max_idx = np.argmax(ang)
d_open = {'force_rad_list': rad[:max_idx+1],
'force_tan_list': tan[:max_idx+1],
'mechanism_x': ang[:max_idx+1], 'mech_type': typ,
'radius': mech_radius,
'time_list': time_list[:max_idx+1]}
if moment_tip != None:
d_open['moment_tip_list'] = moment_tip[:max_idx+1]
d_open['moment_list'] = moment_axis[:max_idx+1]
ut.save_pickle(d_open, opt.dir + '/open_mechanism_trajectories_handhook.pkl')
d_close = {'force_rad_list': rad[max_idx+1:],
'force_tan_list': tan[max_idx+1:],
'mechanism_x': ang[max_idx+1:], 'mech_type': typ,
'radius': mech_radius,
'time_list': time_list[max_idx+1:]}
if moment_tip != None:
d_open['moment_tip_list'] = moment_tip[max_idx+1:]
d_open['moment_list'] = moment_axis[max_idx+1:]
ut.save_pickle(d_close, opt.dir + '/close_mechanism_trajectories_handhook.pkl')
def record_joint_displacements():
print 'hit ENTER to start the recording process'
k = m3t.get_keystroke()
pos_list = []
force_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k = m3t.get_keystroke()
firenze.proxy.step()
pos_list.append(firenze.end_effector_pos('right_arm'))
force_list.append(firenze.get_wrist_force('right_arm',
base_frame=True))
ut.save_pickle({
'pos_list': pos_list,
'force_list': force_list
}, 'stiffness_' + ut.formatted_time() + '.pkl')
firenze.stop()
def simulate_perception(pkls, percep_std, name):
c_list = []
f_list = []
trials_per_pkl = 5
bin_size = math.radians(1.)
max_config = math.radians(100.)
for pkl_nm in pkls:
pull_dict = ut.load_pickle(pkl_nm)
pr2_log = 'pr2' in pkl_nm
for t in range(trials_per_pkl):
radius_err = np.random.normal(scale=percep_std)
#radius_err = np.random.uniform(-percep_std, percep_std)
h_config, h_ftan = online_force_with_radius(
pull_dict, pr2_log, radius_err)
c_list.append(h_config)
f_list.append(h_ftan)
max_config = min(max_config, np.max(h_config))
leng = int(max_config / bin_size) - 1
ff = []
for c, f in zip(c_list, f_list):
c, f = maa.bin(c, f, bin_size, np.mean, False, empty_value=np.nan)
f, c = truncate_to_config(f, c, max_config)
f = np.ma.masked_array(f, np.isnan(f))
f = f[:leng]
c = c[:leng]
ff.append(f)
arr = np.array(ff)
mean = arr.mean(0)
std = arr.std(0)
d = {}
d['std'] = std
d['mean'] = mean
d['rad'] = -3.141592
d['name'] = name
d['config'] = c
d['vec_list'] = arr.tolist()
d['typ'] = 'rotary'
ut.save_pickle(d, name + '.pkl')
def create_time_warped_data(self, subject):
result_list = ['missed', 'good', 'caught_fist', 'caught_other']
velocity_list = ['0.1mps', '0.15mps']
for vel_folder in velocity_list:
for result in result_list:
force_file_list = os.listdir(''.join([self.data_path, '/',subject, '/auto_labeled/', vel_folder, '/', result, '/']))
for f_ind in xrange(100000):
if 'force_profile_'+str(f_ind)+'.pkl' in force_file_list:
load_file = ''.join([self.data_path, '/', subject, '/auto_labeled/', vel_folder, '/', result, '/force_profile_', str(f_ind), '.pkl'])
# print path + vel_folders + "/" + exp + '/' + 'force_profile_'+str(f_ind)+'.pkl'
current_data = load_pickle(load_file)
if vel_folder == '0.1mps':
current_data = self.warp_slow_to_fast(current_data)
save_number = 0
save_file = ''.join([self.data_path, '/', subject, '/time_warped_auto/', vel_folder, '/', result, '/force_profile_', str(save_number), '.pkl'])
while os.path.isfile(save_file):
save_number += 1
save_file = ''.join([self.data_path, '/', subject, '/time_warped_auto/', vel_folder, '/', result, '/force_profile_', str(save_number), '.pkl'])
save_pickle(current_data, save_file)
else:
break
def record_good_configs(use_left_arm):
import m3.toolbox as m3t
import cody_arm_client as cac
if use_left_arm:
arm = 'l'
else:
arm = 'r'
firenze = cac.CodyArmClient(arm)
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
ut.save_pickle(good_configs_list,ut.formatted_time()+'_good_configs_list.pkl')
def select_location(c, thok, angle):
thok.servo.move_angle(angle)
cvim = c.get_frame()
cvim = c.get_frame()
cvim = c.get_frame()
im_angle = thok.servo.read_angle()
tilt_angles = (math.radians(-20) + angle, math.radians(30) + angle)
pos_list, scan_list = thok.scan(tilt_angles, speed=math.radians(10))
m = p3d.generate_pointcloud(pos_list, scan_list, math.radians(-60),
math.radians(60), 0.0, -0.055)
pts = mcf.utmcam0Tglobal(mcf.globalTthok0(m), im_angle)
cam_params = cc.camera_parameters['mekabotUTM']
fx = cam_params['focal_length_x_in_pixels']
fy = cam_params['focal_length_y_in_pixels']
cx, cy = cam_params['optical_center_x_in_pixels'], cam_params[
'optical_center_y_in_pixels']
cam_proj_mat = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
cvim, pts2d = cul.project_points_in_image(cvim, pts, cam_proj_mat)
cp = cul.get_click_location(cvim)
print 'Clicked location:', cp
if cp == None:
return None
idx = cul.get_index(pts2d.T, cp)
pt3d = pts[:, idx]
pt_interest = mcf.globalTutmcam0(pt3d, im_angle)
hl_thok0 = mcf.thok0Tglobal(pt_interest)
l1, l2 = 0.0, -0.055
d = {}
d['pt'] = hl_thok0
d['pos_list'], d['scan_list'] = pos_list, scan_list
d['l1'], d['l2'] = l1, l2
d['img'] = uto.cv2np(cvim)
ut.save_pickle(d, 'hook_plane_scan_' + ut.formatted_time() + '.pkl')
return pt_interest
def param_optimization(self, save_file):
_,n = self.aXData.shape # samples, length
# K-fold CV: Split the dataset in two equal parts
nFold = 8
scores = cross_validation.cross_val_score(self, self.aXData, cv=nFold, n_jobs=-1)
score = -1.0 * sum(scores)/float(len(scores))
# Save data
data = {}
data['score'] = [score]
data['nState'] = self.nState
## data['B'] = self.B
if save_file is None:
save_file='tune_data.pkl'
ut.save_pickle(data, save_file)
return
def generate_library(self):
mytargets = 'all_goals'
mytask = self.task # 'shaving'
#start = 0
#end = 3
myGoals = copy.copy(self.goal_unique)#[self.data_start:self.data_finish]
print 'There are ',len(myGoals),' goals being sent to score generator.'
#print myGoals
selector = ScoreGenerator(visualize=False,targets=mytargets,goals = myGoals,model=self.model)#,tf_listener=self.tf_listener)
#print 'Goals: \n',self.clustered_goal_data[0:4]
#print tft.quaternion_from_matrix(self.clustered_goal_data[0])
score_sheet = selector.handle_score()
#self.score_sheet = []
#for i in xrange(len(score_sheet)):
# self.score_sheet.append([score_sheet[i],reachable[i]])
self.score_sheet = copy.copy(score_sheet)
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('hrl_base_selection')
save_pickle(self.score_sheet,''.join([pkg_path, '/data/',self.model,'_',self.task,'_',mytargets,'_numbers_',str(self.data_start),'_',str(self.data_finish),'_',self.subject,'.pkl']))
if os.path.isfile(''.join([pkg_path, '/data/',self.task,'_score_data.pkl'])):
data1 = load_pickle(''.join([pkg_path, '/data/',self.task,'_score_data.pkl']))
if (np.array_equal(data1[:,0:2],self.score_sheet[:,0:2])):
data1[:,3] += self.score_sheet[:,3]/3
else:
print 'Something is messed up with the score sheets. The saved score sheet does not match up with the new score sheet.'
#for num,i in enumerate(data1):
#data1[num,3]=self.score_sheet[num,3]+i[3]
save_pickle(data1,''.join([pkg_path, '/data/',self.task,'_score_data.pkl']))
print 'There was existing score data for this task. I added the new data to the previous data.'
else:
save_pickle(self.score_sheet,''.join([pkg_path, '/data/',self.task,'_score_data.pkl']))
print 'There was no existing score data for this task. I therefore created a new file.'
q_configs = {}
ee_positions = {}
#for first impact
#key_list = ['start', 'left_start', 'right_start', 'right_to_left_restart', 'left_to_right_restart']
key_list = ['start', 'right_start', 'restart', 'goal']
for key in key_list:
q_configs[key] = []
ee_positions[key] = []
while inp != 'q':
raw_input('press enter to grab current angle for ' + key +
'... \n')
robot.updateHapticState()
joints = robot.joint_angles
ee_position = robot.end_effector_position.reshape(3, 1)
q_configs[key].append(joints)
ee_positions[key].append(ee_position)
print "joints are : ", joints
print "ee_position is :", ee_position
print "\n\n\n\n"
inp = raw_input('press q to quit\n enter to grab next angle:\n')
inp = None
data = {'q_configs': q_configs, 'ee_positions': ee_positions}
ut.save_pickle(data, './starting_configs.pkl')
