def __init__(self,cam): #topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw' not_read = True while not_read: try: cam_info = cam.read_info_data() if(not cam_info == None): not_read = False except: rospy.logerr('info not recieved') self.pcm = PCM() self.cam = cam self.pcm.fromCameraInfo(cam_info) self.br = tf.TransformBroadcaster() self.tl = tf.TransformListener() self.xbox = XboxController()
def __init__(self): self.xbox = XboxController() self.robot = hsrb_interface.Robot() self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') self.pubTwist = rospy.Publisher('hsrb/command_velocity', Twist, queue_size=1)
def __init__(self, com=None, inject_noise=False, noise_scale=1.0): self.alpha = noise_scale self.xbox = XboxController() self.com = com self.i_n = inject_noise if (self.i_n): self.cov_matrix = pickle.load( open(self.com.Options.stats_dir + 'cov_matrix.p', 'rb')) self.pubTwist = rospy.Publisher('hsrb/command_velocity', Twist, queue_size=1) self.record_actions = np.zeros(2) self.curr_action = np.zeros(3) self.twist_applied = None
class JoyStick_X(): def __init__(self, com=None, inject_noise=False, noise_scale=1.0): self.alpha = noise_scale self.xbox = XboxController() self.com = com self.i_n = inject_noise if (self.i_n): self.cov_matrix = pickle.load( open(self.com.Options.stats_dir + 'cov_matrix.p', 'rb')) self.pubTwist = rospy.Publisher('hsrb/command_velocity', Twist, queue_size=1) self.record_actions = np.zeros(2) self.curr_action = np.zeros(3) self.twist_applied = None def apply_control(self): control_state = self.xbox.getControllerState() noise_action = None left_joystick = control_state['left_stick'] right_joystick = control_state['right_stick'] d_pad = control_state['d_pad'] twist = Twist() self.curr_action = np.array( [left_joystick[0], left_joystick[1], right_joystick[1]]) twist = self.com.format_twist(self.curr_action) if (self.i_n and LA.norm(self.curr_action) > 2e-3): noise_action = multivariate_normal(self.curr_action, self.alpha * self.cov_matrix) twist = self.com.format_twist(noise_action) print d_pad self.record_actions = np.array([d_pad[0], d_pad[1]]) self.twist_applied = twist self.pubTwist.publish(twist) return self.curr_action, noise_action, rospy.Time.now() def get_current_control(self): return self.curr_action def get_current_twist(self): return self.twist_applied def get_record_actions(self): return self.record_actions def get_record_actions_passive(self): control_state = self.xbox.getControllerState() d_pad = control_state['d_pad'] return np.array([d_pad[0], d_pad[1]])
class JoyStick_X(): def __init__(self): self.xbox = XboxController() self.robot = hsrb_interface.Robot() self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') self.pubTwist = rospy.Publisher('hsrb/command_velocity', Twist, queue_size=1) def format_twist(self, pos): twist = Twist() gain = -1.0 if (np.abs(pos[1]) < 2e-3): pos[1] = 0.0 twist.linear.x = gain * pos[1] #+ self.noise*normal() twist.linear.y = gain * pos[0] #+ self.noise*normal() twist.angular.z = gain * pos[2] #+ self.noise*normal( return twist def apply_control(self): control_state = self.xbox.getControllerState() noise_action = None left_joystick = control_state['left_stick'] right_joystick = control_state['right_stick'] d_pad = control_state['d_pad'] twist = Twist() self.curr_action = np.array( [left_joystick[0], left_joystick[1], right_joystick[1]]) twist = self.format_twist(self.curr_action) self.record_actions = np.array([d_pad[0], d_pad[1]]) self.twist_applied = twist print twist self.pubTwist.publish(twist) if (self.record_actions[1] < -0.1): print "Move To Home" self.gripper.command(1.2) self.whole_body.move_to_neutral() def get_current_control(self): return self.curr_action def get_current_twist(self): return self.twist_applied def get_record_actions(self): return self.record_actions def get_record_actions_passive(self): control_state = self.xbox.getControllerState() d_pad = control_state['d_pad'] return np.array([d_pad[0], d_pad[1]])
class InitialSampler(object): def __init__(self,cam): #topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw' not_read = True while not_read: try: cam_info = cam.read_info_data() if(not cam_info == None): not_read = False except: rospy.logerr('info not recieved') self.pcm = PCM() self.cam = cam self.pcm.fromCameraInfo(cam_info) self.br = tf.TransformBroadcaster() self.tl = tf.TransformListener() self.xbox = XboxController() def debug_images(self,p_1,p_2): c_img = self.cam.read_color_data() p_1i = (int(p_1[0]),int(p_1[1])) p_2i = (int(p_2[0]),int(p_2[1])) cv2.line(c_img,p_1i,p_2i,(0,0,255),thickness = 10) cv2.imshow('debug',c_img) cv2.waitKey(300) #IPython.embed() def project_to_rgbd(self,trans): M_t = tf.transformations.translation_matrix(trans) M_R = self.get_map_to_rgbd() M_cam_trans = np.matmul(LA.inv(M_R),M_t) return M_cam_trans[0:3,3] def make_projection(self,t_1,t_2): ###GO FROM MAP TO RGBD### t_1 = self.project_to_rgbd(t_1) t_2 = self.project_to_rgbd(t_2) p_1 = self.pcm.project3dToPixel(t_1) p_2 = self.pcm.project3dToPixel(t_2) self.debug_images(p_1,p_2) def debug_broadcast(self,pose,name): while True: self.br.sendTransform((pose[0], pose[1], pose[2]), tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=0.0), rospy.Time.now(), name, #'head_rgbd_sensor_link') 'rgbd_sensor_rgb_frame_map') def get_map_to_rgbd(self): not_found = True while not_found: try: pose = self.tl.lookupTransform('map','rgbd_sensor_rgb_frame_map', rospy.Time(0)) not_found = False except: rospy.logerr("waiting for pose") M = tf.transformations.quaternion_matrix(pose[1]) M_t = tf.transformations.translation_matrix(pose[0]) M[:,3] = M_t[:,3] return M def get_postion(self,name): not_found = True while not_found: try: pose = self.tl.lookupTransform('map',name, rospy.Time(0)) not_found = False except: rospy.logerr("waiting for pose") M = tf.transformations.quaternion_matrix(pose[1]) M_t = tf.transformations.translation_matrix(pose[0]) M[:,3] = M_t[:,3] return pose[0] def look_up_transform(self,count): transforms = self.tl.getFrameStrings() for transform in transforms: current_grasp = 'bed_i_'+str(count) if current_grasp in transform: print 'got here' pose = self.tl.lookupTransform('rgbd_sensor_rgb_frame_map',transform, rospy.Time(0)) M = tf.transformations.quaternion_matrix(pose[1]) M_t = tf.transformations.translation_matrix(pose[0]) M[:,3] = M_t[:,3] return M def sample_corners(self): head_up = self.get_postion("head_up") head_down = self.get_postion("head_down") bottom_up = self.get_postion("bottom_up") bottom_down = self.get_postion("bottom_down") # Get true midpoints of table edges # (i.e. don't assume bottom_up and head_up have the same y value # in case sensor image is tilted) middle_up = np.array([(bottom_up[0] + head_up[0])/2, (bottom_up[1] + head_up[1])/2, bottom_down[2]]) middle_down = np.array([(bottom_down[0] + head_down[0])/2, (bottom_down[1] + head_down[1])/2, bottom_down[2]] ) bottom_middle = np.array([(bottom_down[0] + bottom_up[0])/2, (bottom_down[1] + bottom_up[1])/2, bottom_down[2]] ) head_middle = np.array([(head_down[0] + head_up[0])/2, (head_down[1] + head_up[1])/2, bottom_down[2]]) center = np.array([(bottom_middle[0] + head_middle[0])/2, (middle_down[1] + middle_up[1])/2, bottom_down[2]]) # Generate random point in sensor frame for the closer corner # Draws from gaussian distribution u_down = np.random.uniform(low=0.0,high=0.5) v_down = np.random.uniform(low=0.3,high=1.0) x_down = center[0] + u_down*LA.norm(center - bottom_middle) y_down = center[1] + v_down*LA.norm(center - middle_down) down_corner = (x_down, y_down, center[2]) # Generate random point in sensor frame for the further corner # Draws from gaussian distribution u_up = np.random.uniform(low=0.0,high=0.5) v_up = np.random.uniform(low=0.3,high=1.0) x_up = center[0] - u_up*LA.norm(center - bottom_middle) y_up = center[1] - v_up*LA.norm(center - middle_up) up_corner = (x_up, y_up, center[2]) print "CENTER ",center if center[1] < 0.0 or center[2] < 0.0: raise "ROBOT TRANSFROM INCORRECT" print "UP CORNER ", up_corner print "DOWN CORNER ", down_corner return down_corner, up_corner def sample_initial_state(self): down_corner, up_corner = self.sample_corners() button = 1.0 while button > -0.1: control_state = self.xbox.getControllerState() d_pad = control_state['d_pad'] button = d_pad[1] self.make_projection(down_corner,up_corner) return down_corner, up_corner