class FindObject(): def __init__(self,features,user_name = None): self.com = COM() self.robot = hsrb_interface.Robot() self.noise = 0.1 self.features = features#self.com.binary_image self.count = 0 if(not user_name == None): self.com.Options.setup(self.com.Options.root_dir,user_name) self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') #self.com.go_to_initial_state(self.whole_body,self.gripper) self.joystick = JoyStick_X(self.com) self.tl = TransformListener() self.policy = Policy(self.com,self.features) def run(self): if(self.policy.cam.is_updated): self.com.load_net() self.policy.rollout() self.com.clean_up() self.policy.cam.is_updated = False self.count += 1 def check_initial_state(self): go = False sticky = True while sticky: self.joystick.apply_control() cur_recording = self.joystick.get_record_actions() if(cur_recording[1] > -0.1): print "BEGIN ROLLOUT" sticky = False while not go: img_rgb = self.policy.cam.read_color_data() img_depth = self.policy.cam.read_depth_data() state = self.com.format_data(img_rgb,img_depth) cv2.imshow('initial_state', state[0] ) cv2.waitKey(30) self.joystick.apply_control() cur_recording = self.joystick.get_record_actions() if(cur_recording[1] < -0.1): print "BEGIN ROLLOUT" go = True def go_to_initial_state(self): self.com.go_to_initial_state(self.whole_body,self.gripper) def clear_data(self): self.trajectory = [] def mark_success(self,q): state = {} if(q == 'y'): state['success'] = 1 else: state['success'] = 0 self.trajectory.append(state) def is_goal_objet(self): try: full_pose = self.tl.lookupTransform('head_l_stereo_camera_frame','ar_marker/9', rospy.Time(0)) trans = full_pose[0] transforms = self.tl.getFrameStrings() poses = [] for transform in transforms: if 'bottle' in transform: f_p = self.tl.lookupTransform('head_rgbd_sensor_link',transform, rospy.Time(0)) poses.append(f_p[0]) print 'augmented pose ',f_p print 'ar_marker ', trans for pose in poses: if(LA.norm(pose[2]-0.1-trans[2])< 0.03): return True except: rospy.logerr('AR MARKER NOT THERE') return False def check_success(self): self.com.clean_up() self.b_d = Bottle_Detect(self.policy.cam.read_info_data()) img_rgb = self.policy.cam.read_color_data() img_depth = self.policy.cam.read_depth_data() s_obj,img_detect,poses = self.b_d.detect_bottle(img_rgb,img_depth) success = self.is_goal_objet() self.b_d.clean_up() self.process_data(img_rgb,img_detect,poses,success) print "BOTTLE FOUND ",success return success def process_data(self,img_rgb,img_detect,object_poses,success): img_rgb_cr,img_d = self.com.format_data(img_rgb,None) state = {} state['color_img'] = img_rgb_cr state['found_object'] = img_detect state['object_poses'] = object_poses state['was_success'] = success self.trajectory.append(state) def save_data(self): self.com.save_evaluation(self.trajectory) def check_marker(self): try: A = self.tl.lookupTransform('head_l_stereo_camera_frame','ar_marker/9', rospy.Time(0)) except: rospy.logerr('trash not found') return False return True def execute_grasp(self): self.com.grip_open(self.gripper) self.whole_body.end_effector_frame = 'hand_palm_link' nothing = True try: self.whole_body.move_end_effector_pose(geometry.pose(y=0.15,z=-0.09, ek=-1.57),'ar_marker/9') except: rospy.logerr('mustard bottle found') self.com.grip_squeeze(self.gripper) self.whole_body.move_end_effector_pose(geometry.pose(z=-0.9),'hand_palm_link') self.com.grip_open(self.gripper) self.com.grip_squeeze(self.gripper)
class FindObject(): def __init__(self,features,user_name = None): self.com = COM() self.robot = hsrb_interface.Robot() self.noise = 0.1 self.features = features#self.com.binary_image self.count = 0 if(not user_name == None): self.com.Options.setup(self.com.Options.root_dir,user_name) self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') self.com.go_to_initial_state(self.whole_body,self.gripper) self.joystick = JoyStick_X(self.com) self.policy = Policy(self.com,self.features) self.com.change_grip(self.gripper) def run(self): if(self.policy.cam.is_updated): self.com.load_net() self.policy.rollout() self.com.clean_up() self.policy.cam.is_updated = False self.count += 1 def check_initial_state(self): go = False while not go: img_rgb = self.policy.cam.read_color_data() img_depth = self.policy.cam.read_depth_data() state = self.com.format_data(img_rgb,img_depth) cv2.imshow('initial_state', state[0] ) cv2.waitKey(30) self.joystick.apply_control() cur_recording = self.joystick.get_record_actions() if(cur_recording[1] < -0.1): print "BEGIN ROLLOUT" go = True def go_to_initial_state(self): self.com.go_to_initial_state(self.whole_body,self.gripper) def check_success(self): self.com.clean_up() self.b_d = Bottle_Detect() img_rgb = self.policy.cam.read_color_data() success = self.b_d.detect_bottle(img_rgb) self.b_d.clean_up() print "BOTTLE FOUND ",success return success def save_data(self,img_rgb,img_detect): state = self.com.format_data(img_rgb,None) state['found_object'] = img_detect state['object_poses'] def check_marker(self): try: A = self.tl.lookupTransform('head_l_stereo_camera_frame','ar_marker/12', rospy.Time(0)) except: rospy.logerr('trash not found') return False return True def execute_grasp(self): self.com.grip_open(self.gripper) self.whole_body.end_effector_frame = 'hand_palm_link' nothing = True try: self.whole_body.move_end_effector_pose(geometry.pose(y=0.15,z=-0.11, ek=-1.57),'ar_marker/9') nothing = False except: rospy.logerr('mustard bottle found') self.com.grip_squeeze(self.gripper) self.whole_body.move_end_effector_pose(geometry.pose(z=-0.9),'hand_palm_link') self.com.grip_open(self.gripper) self.com.grip_squeeze(self.gripper)
class Compute_Noise(): def __init__(self, user_name=None): self.noise = 0.1 self.start_recording = False self.stop_recording = False self.com = COM(load_net=True) if (not user_name == None): self.com.Options.setup(self.com.Options.root_dir, user_name) self.com.load_net() self.options = self.com.Options self.features = Features() def get_test_train_split(self): train_labels, test_labels = pickle.load( open(self.options.stats_dir + 'test_train_f.p', 'r')) self.test_trajs = [] for filename in test_labels: rollout_data = pickle.load( open(self.options.rollouts_dir + filename + '/rollout.p', 'r')) self.test_trajs.append(rollout_data) def compute_covariance_matrix(self): self.covariance = np.zeros([3, 3]) self.get_test_train_split() N = len(self.test_trajs) for traj in self.test_trajs: T = float(len(traj)) t_covariance = np.zeros([3, 3]) for state in traj: img = state['color_img'] action = state['action'] action_ = self.com.eval_policy(img, self.features.vgg_features, cropped=True) action_f = np.zeros([3, 1]) action_f[:, 0] = (action - action_) rank_one_update = action_f * action_f.T #IPython.embed() t_covariance = t_covariance + rank_one_update self.covariance = t_covariance * (1.0 / T) + self.covariance self.covariance = self.covariance * (1.0 / N) print "COMPUTED COVARIANCE MATRIX" print self.covariance def save_covariance_matrix(self): pickle.dump(self.covariance, open(self.options.stats_dir + 'cov_matrix.p', 'wb'))