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