def __init__(self): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.cam = RGBD() self.com = COM() not_read = True while not_read: try: cam_info = self.cam.read_info_data() if (not cam_info == None): not_read = False except: rospy.logerr('info not recieved') #self.side = 'BOTTOM' self.cam_info = cam_info self.cam = RGBD() self.com = COM()
def __init__(self): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.robot = hsrb_interface.Robot() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.whole_body.move_to_joint_positions({'head_tilt_joint': -0.8}) self.cam = RGBD() self.wl = Python_Labeler(cam=self.cam) self.gp = GraspPlanner()
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,com,features): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.pubTwist = rospy.Publisher('/hsrb/command_velocity', Twist, queue_size=1) self.com = com self.cam = RGBD() self.trajectory = [] self.features = features
def __init__(self): self.robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.side = 'BOTTOM' self.cam = RGBD() self.com = COM() self.com.go_to_initial_state(self.whole_body) self.grasp_count = 0 self.br = tf.TransformBroadcaster() self.tl = TransformListener() self.gp = GraspPlanner() self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('gripper')) self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('suction')) self.gm = GraspManipulator(self.gp, self.gripper, self.suction, self.whole_body, self.omni_base, self.tl) self.web = Web_Labeler() print "after thread"
def __init__(self): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') #PARAMETERS TO CHANGE self.side = 'TOP' self.r_count = 0 self.grasp_count = 0 self.success_count = 0 self.true_count = 0 self.grasp = True self.r_count = self.get_rollout_number() self.cam = RGBD() self.com = COM() self.joystick = JoyStick_X(self.com) if cfg.USE_WEB_INTERFACE: self.wl = Web_Labeler() else: self.wl = Python_Labeler(cam=self.cam) self.com.go_to_initial_state(self.whole_body) self.tt = TableTop() self.tt.find_table(self.robot) self.position_head() self.br = tf.TransformBroadcaster() self.tl = TransformListener() #self.test_current_point() time.sleep(4) #thread.start_new_thread(self.ql.run,()) print "after thread"
def __init__(self): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.robot = hsrb_interface.Robot() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.cam = RGBD() self.com = COM() #self.com.go_to_initial_state(self.whole_body) self.count = 425 self.joystick = JoyStick_X(self.com) self.true_count = 0
def __init__(self): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.robot = hsrb_interface.Robot() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.cam = RGBD() self.com = COM() self.com.go_to_initial_state(self.whole_body) self.br = tf.TransformBroadcaster() self.tl = TransformListener() self.gp = GraspPlanner() self.suction = Suction(self.gp, self.cam) #thread.start_new_thread(self.ql.run,()) print "after thread"
def __init__(self): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.side = 'BOTTOM' self.cam = RGBD() self.com = COM() if cfg.USE_WEB_INTERFACE: self.wl = Web_Labeler() else: self.wl = Python_Labeler(self.cam) self.com.go_to_initial_state(self.whole_body) self.tt = TableTop() self.tt.find_table(self.robot) self.ins = InitialSampler(self.cam) self.grasp_count = 0 self.br = tf.TransformBroadcaster() self.tl = TransformListener() self.gp = GraspPlanner() self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('gripper')) self.g_detector = Analytic_Grasp() self.sn = Success_Net(self.whole_body, self.tt, self.cam, self.omni_base) c_img = self.cam.read_color_data() #self.test_current_point() time.sleep(4) #thread.start_new_thread(self.ql.run,()) print "after thread"
def __init__(self, args): """For data collection of bed-making, NOT the deployment. Assumes we roll out the robot's policy via code (not via human touch). This is the 'slower' way where we have the python interface that the human clicks on to indicate grasping points. Good news is, our deployment code is probably going to be similar to this. For joystick: you only need it plugged in for the initial state sampler, which (at the moment) we are not even using. """ self.robot = robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = robot.get('omni_base') self.whole_body = robot.get('whole_body') self.cam = RGBD() self.com = COM() self.wl = Python_Labeler(cam=self.cam) # View mode: STANDARD (the way I was doing earlier), CLOSE (the way they want). self.view_mode = cfg.VIEW_MODE # Set up initial state, table, etc. self.com.go_to_initial_state(self.whole_body) self.tt = TableTop() # For now, a workaround. Ugly but it should do the job ... #self.tt.find_table(robot) self.tt.make_fake_ar() self.tt.find_table_workaround(robot) #self.ins = InitialSampler(self.cam) self.side = 'BOTTOM' self.grasp_count = 0 # Bells and whistles; note the 'success check' to check if transitioning self.br = tf.TransformBroadcaster() self.tl = TransformListener() self.gp = GraspPlanner() self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options, robot.get('gripper')) self.sc = Success_Check(self.whole_body, self.tt, self.cam, self.omni_base) time.sleep(4) print( "Finished creating BedMaker()! Get the bed set up and run bed-making!" ) if cfg.INS_SAMPLE: print("TODO: we don't have sampling code here.") # When we start, spin this so we can check the frames. Then un-comment, # etc. It's the current hack we have to get around crummy AR marker detection. if args.phase == 1: print("Now doing rospy.spin() because phase = 1.") rospy.spin()
def __init__(self): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.side = 'BOTTOM' self.cam = RGBD() self.com = COM() # if cfg.USE_WEB_INTERFACE: # self.wl = Web_Labeler() # else: # self.wl = Python_Labeler(cam = self.cam) self.com.go_to_initial_state(self.whole_body) self.tt = TableTop() self.tt.find_table(self.robot) self.grasp_count = 0 self.br = tf.TransformBroadcaster() self.tl = TransformListener() self.gp = GraspPlanner() self.gripper = Lego_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('gripper')) self.RCNN = Depth_Object("bottle") #self.test_current_point() #thread.start_new_thread(self.ql.run,()) print "after thread"
def __init__(self): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' self.robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.side = 'BOTTOM' self.cam = RGBD() self.com = COM() if not DEBUG: self.com.go_to_initial_state(self.whole_body) self.tt = TableTop() self.tt.find_table(self.robot) self.wl = Python_Labeler(cam=self.cam) self.grasp_count = 0 self.br = tf.TransformBroadcaster() self.tl = TransformListener() self.ds = data_saver('tpc_rollouts/rollouts') self.gp = GraspPlanner() self.gripper = Crane_Gripper(self.gp, cam, options, robot.get('gripper')) self.gm = GraspManipulator(self.gp, self.gripper, self.whole_body, self.omni_base, self.tt) print "after thread"
def __init__(self): self.cam = RGBD() not_read = True while not_read: try: cam_info = self.cam.read_info_data() if (not cam_info == None): not_read = False except: rospy.logerr('info not recieved') self.pcm = PCM() self.pcm.fromCameraInfo(cam_info)
def __init__(self): """ Class to run HSR lego task """ self.robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.side = 'BOTTOM' self.cam = RGBD() self.com = COM() # if not DEBUG: self.com.go_to_initial_state(self.whole_body) # self.tt = TableTop() # self.tt.find_table(self.robot) self.grasp_count = 0 self.br = tf.TransformBroadcaster() self.tl = TransformListener() self.gp = GraspPlanner() self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('gripper')) self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('suction')) self.gm = GraspManipulator(self.gp, self.gripper, self.suction, self.whole_body, self.omni_base, self.tl) self.collision_world = hsrb_interface.collision_world.CollisionWorld( "global_collision_world") self.collision_world.remove_all() self.collision_world.add_box(x=.8, y=.9, z=0.5, pose=geometry.pose(y=1.4, z=0.15), frame_id='map') print "after thread"
def __init__(self): """ Class to run HSR lego task """ self.robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.side = 'BOTTOM' self.cam = RGBD() self.com = COM() self.com.go_to_initial_state(self.whole_body) self.grasp_count = 0 self.helper = Helper(cfg) self.br = tf.TransformBroadcaster() self.tl = TransformListener() self.gp = GraspPlanner() self.gripper = Crane_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('gripper')) self.suction = Suction_Gripper(self.gp, self.cam, self.com.Options, self.robot.get('suction')) self.gm = GraspManipulator(self.gp, self.gripper, self.suction, self.whole_body, self.omni_base, self.tl) self.dl = DataLogger("stats_data/model_base", cfg.EVALUATE) self.web = Web_Labeler(cfg.NUM_ROBOTS_ON_NETWORK) model_path = 'main/output_inference_graph.pb' label_map_path = 'main/object-detection.pbtxt' self.det = Detector(model_path, label_map_path) print "after thread"
def __init__(self,options,name = None): ''' Initialization class for a Policy Parameters ---------- yumi : An instianted yumi robot com : The common class for the robot cam : An open bincam class debug : bool A bool to indicate whether or not to display a training set point for debuging. ''' if(name == None): name = '07_14_15_27_17save.ckpt-12000' self.cam = RGBD() not_read = True while not_read: try: cam_info = self.cam.read_info_data() if(not cam_info == None): not_read = False except: rospy.logerr('info not recieved') self.pcm = PCM() self.pcm.fromCameraInfo(cam_info) self.options = options self.detect = Detector(name) self.br = tf.TransformBroadcaster() self.gp = GraspPlanner()
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()
T = 40 setup_dir = "bed_making/" root_dir = "/media/autolab/1tb/" def __init__(self): self.setup(self.root_dir, self.setup_dir) if __name__ == '__main__': #hsrb_interface.Robot() rospy.init_node('readJoy_node', anonymous=True) cam = RGBD() c_o = Corl_Options() count = 0 while True: rgb_img = cam.read_color_data() count += 1 if (not rgb_img == None): img_cropped = rgb_img[c_o.OFFSET_X:c_o.OFFSET_X + c_o.WIDTH, c_o.OFFSET_Y:c_o.OFFSET_Y + c_o.HEIGHT, :] cv2.imshow('debug', img_cropped)
def __init__(self, args): """For deploying the bed-making policy, not for data collection. We use all three variants (analytic, human, networks) here due to similarities in code structure. """ self.args = args DEBUG = True # Set up the robot. self.robot = robot = hsrb_interface.Robot() if DEBUG: print("finished: hsrb_interface.Robot()...") self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') if DEBUG: print("finished: robot.get(omni_base)...") self.whole_body = self.robot.get('whole_body') if DEBUG: print("finished: robot.get(whole_body)...") self.cam = RGBD() self.com = COM() self.wl = Python_Labeler(cam=self.cam) # Set up initial state, table, etc. Don't forget view mode! self.view_mode = BED_CFG.VIEW_MODE self.com.go_to_initial_state(self.whole_body) if DEBUG: print("finished: go_to_initial_state() ...") self.tt = TableTop() if DEBUG: print("finished: TableTop()...") # For now, a workaround. Ugly but it should do the job ... #self.tt.find_table(robot) self.tt.make_fake_ar() self.tt.find_table_workaround(robot) #self.ins = InitialSampler(self.cam) self.side = 'BOTTOM' self.grasp_count = 0 self.b_grasp_count = 0 self.t_grasp_count = 0 # AH, build the YOLO network beforehand. g_cfg = BED_CFG.GRASP_CONFIG s_cfg = BED_CFG.SUCC_CONFIG self.yc = YOLO_CONV(options=s_cfg) self.yc.load_network() # Policy for grasp detection, using Deep Imitation Learning. # Or, actually, sometimes we will use humans or an analytic version. if DEBUG: self._test_variables() print("\nnow forming the GDetector with type {}".format(args.g_type)) if args.g_type == 'network': self.g_detector = GDetector(g_cfg, BED_CFG, yc=self.yc) elif args.g_type == 'analytic': self.g_detector = Analytic_Grasp() # TODO not implemented! elif args.g_type == 'human': print("Using a human, don't need to have a `g_detector`. :-)") if DEBUG: self._test_variables() print("\nnow making success net") self.sn = Success_Net(self.whole_body, self.tt, self.cam, self.omni_base, fg_cfg=s_cfg, bed_cfg=BED_CFG, yc=self.yc) # Bells and whistles. self.br = TransformBroadcaster() self.tl = TransformListener() self.gp = GraspPlanner() self.gripper = Bed_Gripper(self.gp, self.cam, self.com.Options, robot.get('gripper')) self.dp = DrawPrediction() # When we start, do rospy.spin() to check the frames (phase 1). Then re-run. # The current hack we have to get around crummy AR marker detection. :-( if DEBUG: self._test_variables() print("Finished with init method") time.sleep(4) if args.phase == 1: print("Now doing rospy.spin() because phase = 1.") rospy.spin() # For evaluating coverage. self.img_start = None self.img_final = None self.img_start2 = None self.img_final2 = None # For grasp offsets. self.apply_offset = False
def __init__(self): """ For faster data collection where we manually simulate it. We move with our hands. This will give us the large datasets we need. Supports both grasping and success net data collection. If doing the grasping, DON'T MAKE IT A SUCCESS CASE where the blanket is all the way over the corner. That way we can use the images for both grasping and as failure cases for the success net. For the success net data collection, collect data at roughly a 5:1 ratio of successes:failures, and make failures the borderline cases. Then we borrow data from the grasping network to make it 5:5 or 1:1 for the actual success net training process (use another script for forming the data). We use the keys on the joystick to indicate the success/failure class. """ makedirs() self.robot = robot = hsrb_interface.Robot() self.rgbd_map = RGBD2Map() self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.cam = RGBD() self.com = COM() self.wl = Python_Labeler(cam=self.cam) # ---------------------------------------------------------------------- # PARAMETERS TO CHANGE (well, really the 'side' and 'grasp' only). # We choose a fixed side and collect data from there, no switching. # Automatically saves based on `r_count` and counting the saved files. # `self.grasp` remains FIXED in the code, so we're either only # collecting grasp or only collecting success images. # ---------------------------------------------------------------------- self.side = 'BOTTOM' # CHANGE AS NEEDED self.grasp = False # CHANGE AS NEEDED self.grasp_count = 0 self.success_count = 0 self.true_count = 0 self.r_count = self.get_rollout_number() self.joystick = JoyStick_X(self.com) print("NOTE: grasp={} (success={}), side: {}, rollout num: {}".format( self.grasp, not self.grasp, self.side, self.r_count)) print("Press X for any SUCCESS (class 0), Y for FAILURES (class 1).") # Set up initial state, table, etc. self.com.go_to_initial_state(self.whole_body) self.tt = TableTop() # For now, a workaround. Ugly but it should do the job ... #self.tt.find_table(robot) self.tt.make_fake_ar() self.tt.find_table_workaround(robot) self.br = tf.TransformBroadcaster() self.tl = TransformListener() time.sleep(4) # When we start, spin this so we can check the frames. Then un-comment, # etc. It's the current hack we have to get around crummy AR marker detection. #rospy.spin() # THEN we position the head since that involves moving the _base_. self.position_head()
def clean_up(self): self.sess.close() tfl.reset_default_graph() def broadcast_poses(self, poses): count = 0 for pose in poses: print "POSE ", pose #IPython.embed() self.br.sendTransform((pose[0], pose[1] - 0.05, pose[2] + 0.06), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), 'bottle' + str(count), 'head_rgbd_sensor_link') count += 1 if __name__ == '__main__': rospy.init_node('readJoy_node', anonymous=True) print 'bottle' rgbd = RGBD() time.sleep(5) do = Bottle_Detect() while True: print "IMGAE" print rgbd.read_color_data() if (not rgbd.read_color_data() == None): do.detect_bottle(rgbd.read_color_data())
# make save dir if not os.path.exists(SAVE_PATH): os.makedirs(SAVE_PATH) # Orient the robot appropriately. make sure it is starting about a foot away from the long side of the bed, facing parallel to the bed robot = hsrb_interface.Robot() whole_body = robot.get('whole_body') whole_body.move_to_go() whole_body.move_to_joint_positions({'arm_flex_joint': -np.pi / 16.0}) whole_body.move_to_joint_positions({'head_pan_joint': np.pi / 2.0}) whole_body.move_to_joint_positions({'head_tilt_joint': -np.pi / 4.0}) whole_body.move_to_joint_positions({'arm_lift_joint': 0.120}) # start collecting data rollout_num = 1 # do about 120 rollouts for decent dataset camera = RGBD() labels = list() userinput = 'a' while userinput != 'q': print("rollout", rollout_num) # generate random numbers for top/bottom, success/failure of rollout top = np.random.randint(2) success = np.random.randint(2) labels.append(str(success)) # classification labels print("Top (1)/Bottom (0): ", top) print("Success (1)/Failure (0): ", success) userinput = raw_input( "press enter to take the picture, or enter q to quit: ") if userinput != 'q': c_img = camera.read_color_data() d_img = camera.read_depth_data()