def __init__(self, user_name=None, inject_noise=False, noise_scale=1.0): self.robot = hsrb_interface.Robot() self.noise = 0.1 self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') self.tl = TransformListener() self.cam = RGBD() time.sleep(5) self.b_d = Bottle_Detect(self.cam.read_info_data()) self.start_recording = False self.stop_recording = False self.com = COM() if (not user_name == None): self.com.Options.setup(self.com.Options.root_dir, user_name) #self.com.go_to_initial_state(self.whole_body,self.gripper) #self.whole_body.move_to_joint_positions({'head_tilt_joint':-0.3}) self.joystick = JoyStick_X(self.com, inject_noise=inject_noise, noise_scale=noise_scale) self.torque = Gripper_Torque() self.joints = Joint_Positions()
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 __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 __init__(self): self.robot = hsrb_interface.Robot() self.noise = 0.1 self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') self.tl = TransformListener() self.start_recording = False self.stop_recording = False self.com = COM() self.com.go_to_initial_state(self.whole_body, self.gripper) #self.whole_body.move_to_joint_positions({'head_tilt_joint':-0.3}) self.cam = RGBD() self.joystick = JoyStick() self.torque = Gripper_Torque() self.joints = Joint_Positions()
if args.last is not None: last = args.last else: print "please enter a last value with -l (not inclusive)" sys.exit() f = [] for (dirpath, dirnames, filenames) in os.walk( Options.rollouts_dir): #specific: sup_dir from specific options print dirpath print filenames f.extend(dirnames) train_data = [] test_data = [] com = COM() image_data = [] count = 0 for filename in f: rollout_data = pickle.load( open(Options.rollouts_dir + filename + '/rollout.p', 'r')) state = rollout_data[0] img = state['color_img'] com.depth_state(state) height, width, layers = img.shape
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)
sys.exit() Options.setup(Options.root_dir,user_name) f = [] for (dirpath, dirnames, filenames) in os.walk(Options.rollouts_dir): #specific: sup_dir from specific options print dirpath print filenames f.extend(dirnames) train_data = [] test_data = [] com = COM() for filename in f: if( filename == 'rollout42'): rollout_data = pickle.load(open(Options.rollouts_dir+filename+'/rollout.p','r')) state = rollout_data[0] img = state['color_img'] com.depth_state(state) height,width,layers = img.shape a = cv2.cv.CV_FOURCC('M','J','P','G')
class Collect_Demos(): def __init__(self): self.robot = hsrb_interface.Robot() self.noise = 0.1 self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') self.tl = TransformListener() self.start_recording = False self.stop_recording = False self.com = COM() self.com.go_to_initial_state(self.whole_body, self.gripper) #self.whole_body.move_to_joint_positions({'head_tilt_joint':-0.3}) self.cam = RGBD() self.joystick = JoyStick() self.torque = Gripper_Torque() self.joints = Joint_Positions() def proess_state(self): img_rgb = self.cam.read_color_data() img_depth = self.cam.read_depth_data() cur_action = self.joystick.get_current_control() state = self.com.format_data(img_rgb, img_depth) cv2.imshow('debug', state[0]) cv2.waitKey(30) #Save all data data = {} data['action'] = cur_action data['color_img'] = state[0] data['depth_img'] = state[1] data['noisey_twist'] = self.joystick.get_current_twist() data['gripper_torque'] = self.torque.read_data() data['joint_positions'] = self.joints.read_data() if (LA.norm(cur_action) > 1e-4): self.trajectory.append(data) def run(self): cur_recording = self.joystick.get_record_actions() if (cur_recording[0] < -0.1): print "BEGIN DATA COLLECTION" self.start_recording = True count = 0 if (self.start_recording): self.trajectory = [] while not self.stop_recording: #while count < 20: if (count % 6 == 0): self.proess_state() cur_recording = self.joystick.get_record_actions() if (cur_recording[1] < -0.1): print "END DATA COLLECTION" self.stop_recording = True count += 1 q = input('SAVE DATA: y or n: ') if (q == 'y'): self.com.save_recording(self.trajectory) self.start_recording = False self.stop_recording = False
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 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'))
from il_ros_hsr.p_pi.safe_corl.com import Safe_COM as COM import rospy import time #specific: fetches specific net file #from deep_lfd.tensor.nets.net_grasp import Net_Grasp as Net ######################################################## if __name__ == '__main__': robot = hsrb_interface.Robot() Options = options() yolo_detect = YoloDetect(Options) com = COM() omni_base = robot.get('omni_base') whole_body = robot.get('whole_body') gripper = robot.get('gripper') #com.go_to_initial_state(whole_body,gripper) cam = RGBD() time.sleep(3) while True: c_img = cam.read_color_data() d_img = cam.read_depth_data()
class Collect_Demos(): def __init__(self, user_name=None, inject_noise=False, noise_scale=1.0): self.robot = hsrb_interface.Robot() self.noise = 0.1 self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') self.tl = TransformListener() self.cam = RGBD() time.sleep(5) self.b_d = Bottle_Detect(self.cam.read_info_data()) self.start_recording = False self.stop_recording = False self.com = COM() if (not user_name == None): self.com.Options.setup(self.com.Options.root_dir, user_name) #self.com.go_to_initial_state(self.whole_body,self.gripper) #self.whole_body.move_to_joint_positions({'head_tilt_joint':-0.3}) self.joystick = JoyStick_X(self.com, inject_noise=inject_noise, noise_scale=noise_scale) self.torque = Gripper_Torque() self.joints = Joint_Positions() def proess_state(self): img_rgb = self.cam.read_color_data() img_depth = self.cam.read_depth_data() cur_action, noise_action, time_pub = self.joystick.apply_control() state = self.com.format_data(img_rgb, img_depth) #cv2.imwrite('frame_'+str(self.count)+'.png',state[0]) #Save all data data = {} data['action'] = cur_action data['color_img'] = state[0] data['depth_img'] = state[1] data['noise_action'] = noise_action pose = self.whole_body.get_end_effector_pose().pos pose = np.array([pose.x, pose.y, pose.z]) data['robot_pose'] = pose # data['action_time'] = time_pub # data['image_time'] = self.cam.color_time_stamped print "ACTION AT COUNT ", self.count print cur_action self.count += 1 self.trajectory.append(data) # if(LA.norm(cur_action) > 1e-3): # print "GOT ACCEPTED" # self.trajectory.append(data) def check_success(self): img_rgb = self.cam.read_color_data() img_depth = self.cam.read_depth_data() success = self.b_d.detect_bottle(img_rgb, img_depth) print "BOTTLE FOUND ", success return success def run(self): self.joystick.apply_control() cur_recording = self.joystick.get_record_actions_passive() if (cur_recording[0] < -0.1): print "BEGIN DATA COLLECTION" self.start_recording = True count = 0 if (self.start_recording): self.count = 0 self.trajectory = [] while not self.stop_recording: #while count < 20: if (self.cam.is_updated): self.proess_state() self.cam.is_updated = False cur_recording = self.joystick.get_record_actions() if (cur_recording[1] < -0.1): print "END DATA COLLECTION" self.stop_recording = True count += 1 self.check_success() q = input('SAVE DATA: y or n: ') if (q == 'y'): self.com.save_recording(self.trajectory) self.start_recording = False self.stop_recording = False