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 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')