ravens/tasks/defs_bags.py
def determine_task_stage(self, colormap=None, heightmap=None,
                             object_mask=None, visible_beads=None):
        """Get the task stage in a consistent manner among different policies.

        When training an oracle policy, we can determine the training stage,
        which is critical because of this task's particular quirks in
        requiring different action parameters (particularly height of the
        pull) for each stage. One option is to use this method to determine
        the hard-coded task stage for each task. This does depend on the
        learned policy inferring when to switch among task stages?

        For policies, I use simpler methods in their classes. They don't call this.

        Returns: False if the task is almost certainly going to fail.
        """
        if self.task_stage == 2 and (len(self.items_in_bag_IDs) == len(self.item_IDs)):
            #print('now on task stage 2 --> 3')
            self.task_stage = 3
            return (True, None)
        elif self.task_stage == 3:
            return (True, None)

        # Hand-tuned, if too small the agent won't open the bag enough ...
        BUF = 0.025

        # But we can decrease it if we're on task stage 2 and have to put in more items.
        if self.task_stage == 2:
            BUF = 0.015

        # Check object_mask for all IDs that correspond to the cable ring.
        cable_IDs = np.array(self.cable_bead_IDs)
        bead_mask = np.isin(object_mask, test_elements=cable_IDs)

        # Threshold image to get 0s and 255s (255s=bead pixels) and find its contours.
        bead_mask = np.uint8(bead_mask * 255)
        contours, _ = cv2.findContours(bead_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        # If only a few beads are visible (or no contours detected) exit early.
        frac_visible = len(visible_beads) / len(self.cable_bead_IDs)
        if len(contours) == 0 or frac_visible <= BEAD_THRESH:
            return (False, None)

        # Combine contours via concatenation (shape=(N,1,2)) and get the convex hull.
        allc = np.concatenate([c for c in contours])
        contours_list = [allc]
        hull_list = [cv2.convexHull(c) for c in contours_list]

        # Make an RGB image, then draw the filled-in area of all items in `hull_list`.
        hull = np.zeros((bead_mask.shape[0], bead_mask.shape[1], 3), dtype=np.uint8)
        cv2.drawContours(hull, hull_list, -1, (255,255,255), thickness=-1)
        hull = cv2.cvtColor(hull, cv2.COLOR_BGR2GRAY)

        # Following task.random_pose, use object_size to find placing points. Assumes
        # object sizes are same. We add a buffer since convex hulls inflate area.
        # TODO(daniel) this is where we should really be using rotation.
        object_size = self.item_sizes[0]
        object_size = (object_size[0] + BUF, object_size[1] + BUF, object_size[2])
        max_size = np.sqrt(object_size[0]**2 + object_size[1]**2)
        erode_size = int(np.round(max_size / self.pixel_size))

        if self.task_stage == 2 and len(self.items_in_bag_IDs) > 0:
            # For the hard bag-items version, get array of 0s = items in bag (hence,
            # invalid placing points) and 1s = all other points (could be valid).
            pixels_bag_items = np.ones((hull.shape[0], hull.shape[1]), dtype=np.uint8)
            for item_id in self.items_in_bag_IDs:
                item_pix = np.uint8(item_id == object_mask)
                pixels_bag_items = pixels_bag_items & item_pix  # Logical AND
            pixels_no_bag_items = np.uint8(1 - pixels_bag_items)
        else:
            # Make it all 1s so it's safe to apply logical AND with hull pixels.
            pixels_no_bag_items = np.ones((hull.shape[0], hull.shape[1]), dtype=np.uint8)

        # Combine the hull and pixel conditions.
        place_pixels_hull = np.uint8(hull == 255)
        place_pixels = place_pixels_hull & pixels_no_bag_items

        # Use cv2.erode to find valid place pixels.
        kernel = np.ones((erode_size, erode_size), np.uint8)
        place_pixels_eroded = cv2.erode(place_pixels, kernel)

        # If we're in task stage 2 and there's nothing, let's revert back to original.
        if self.task_stage == 2 and np.sum(place_pixels_eroded) == 0:
            place_pixels_eroded = cv2.erode(place_pixels_hull, kernel)

        # Keep this debugging code to make it easier to inspect.
        if self._debug:
            heightmap = heightmap / np.max(heightmap) * 255
            place_rgb = cv2.cvtColor(hull.copy(), cv2.COLOR_GRAY2BGR)
            place_rgb[place_pixels_eroded > 0] = 127  # gray
            print(f'max_size: {max_size:0.3f}, erode_size: {erode_size}')
            print(f'number of pixels for placing: {np.sum(place_pixels)}')
            print(f'number of pixels for placing (after eroding): {np.sum(place_pixels_eroded)}')
            nb = len([x for x in os.listdir('tmp/') if 'color' in x and '.png' in x])
            cv2.imwrite(f'tmp/img_{nb}_colormap.png', cv2.cvtColor(colormap, cv2.COLOR_RGB2BGR).astype(np.uint8))
            cv2.imwrite(f'tmp/img_{nb}_heightmap.png', heightmap.astype(np.uint8))
            cv2.imwrite(f'tmp/img_{nb}_bead_mask.png', bead_mask)
            cv2.imwrite(f'tmp/img_{nb}_place_rgb.png', cv2.cvtColor(place_rgb, cv2.COLOR_RGB2BGR))
            cv2.imwrite(f'tmp/img_{nb}_place_pixels_eroded.png', np.uint8(place_pixels_eroded * 255))
            if self.task_stage == 2 and len(self.items_in_bag_IDs) > 0:
                pixels_no_bag_items *= 255
                cv2.imwrite(f'tmp/img_{nb}_pixels_no_bag_items.png', np.uint8(pixels_no_bag_items))

        # If on stage 1, and there exists any possible placing point, go to stage 2.
        if self.task_stage == 1:
            if np.sum(place_pixels_eroded) > 0:
                self.task_stage = 2
                #print('now on task stage 1 --> 2')
        return (True, place_pixels_eroded)
Get the task stage in a consistent manner among different policies.

        When training an oracle policy, we can determine the training stage,
        which is critical because of this task's particular quirks in
        requiring different action parameters (particularly height of the
        pull) for each stage. One option is to use this method to determine
        the hard-coded task stage for each task. This does depend on the
        learned policy inferring when to switch among task stages?

        For policies, I use simpler methods in their classes. They don't call this.

        Returns: False if the task is almost certainly going to fail.
        
Get the task stage in a consistent manner among different policies.
When training an oracle policy, we can determine the training stage,
which is critical because of this task's particular quirks in
requiring different action parameters (particularly height of the
pull) for each stage. One option is to use this method to determine
the hard-coded task stage for each task.

For policies, I use simpler methods in their classes. They don't call this.

False if the task is almost certainly going to fail.
Get the task stage in a consistent manner among different policies . When training an oracle policy we can determine the training stage which is critical because of this task ' s particular quirks in requiring different action parameters ( particularly height of the pull ) for each stage . One option is to use this method to determine the hard - coded task stage for each task . For policies I use simpler methods in their classes . They don ' t call this . False if the task is almost certainly going to fail .
def determine_task_stage(self, colormap=None, heightmap=None,
                             object_mask=None, visible_beads=None):
        if self.task_stage == 2 and (len(self.items_in_bag_IDs) == len(self.item_IDs)):
            self.task_stage = 3
            return (True, None)
        elif self.task_stage == 3:
            return (True, None)
        BUF = 0.025
        if self.task_stage == 2:
            BUF = 0.015
        cable_IDs = np.array(self.cable_bead_IDs)
        bead_mask = np.isin(object_mask, test_elements=cable_IDs)
        bead_mask = np.uint8(bead_mask * 255)
        contours, _ = cv2.findContours(bead_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        frac_visible = len(visible_beads) / len(self.cable_bead_IDs)
        if len(contours) == 0 or frac_visible <= BEAD_THRESH:
            return (False, None)
        allc = np.concatenate([c for c in contours])
        contours_list = [allc]
        hull_list = [cv2.convexHull(c) for c in contours_list]
        hull = np.zeros((bead_mask.shape[0], bead_mask.shape[1], 3), dtype=np.uint8)
        cv2.drawContours(hull, hull_list, -1, (255,255,255), thickness=-1)
        hull = cv2.cvtColor(hull, cv2.COLOR_BGR2GRAY)
        object_size = self.item_sizes[0]
        object_size = (object_size[0] + BUF, object_size[1] + BUF, object_size[2])
        max_size = np.sqrt(object_size[0]**2 + object_size[1]**2)
        erode_size = int(np.round(max_size / self.pixel_size))
        if self.task_stage == 2 and len(self.items_in_bag_IDs) > 0:
            pixels_bag_items = np.ones((hull.shape[0], hull.shape[1]), dtype=np.uint8)
            for item_id in self.items_in_bag_IDs:
                item_pix = np.uint8(item_id == object_mask)
                pixels_bag_items = pixels_bag_items & item_pix  
            pixels_no_bag_items = np.uint8(1 - pixels_bag_items)
        else:
            pixels_no_bag_items = np.ones((hull.shape[0], hull.shape[1]), dtype=np.uint8)
        place_pixels_hull = np.uint8(hull == 255)
        place_pixels = place_pixels_hull & pixels_no_bag_items
        kernel = np.ones((erode_size, erode_size), np.uint8)
        place_pixels_eroded = cv2.erode(place_pixels, kernel)
        if self.task_stage == 2 and np.sum(place_pixels_eroded) == 0:
            place_pixels_eroded = cv2.erode(place_pixels_hull, kernel)
        if self._debug:
            heightmap = heightmap / np.max(heightmap) * 255
            place_rgb = cv2.cvtColor(hull.copy(), cv2.COLOR_GRAY2BGR)
            place_rgb[place_pixels_eroded > 0] = 127  
            print(f'max_size: {max_size:0.3f}, erode_size: {erode_size}')
            print(f'number of pixels for placing: {np.sum(place_pixels)}')
            print(f'number of pixels for placing (after eroding): {np.sum(place_pixels_eroded)}')
            nb = len([x for x in os.listdir('tmp/') if 'color' in x and '.png' in x])
            cv2.imwrite(f'tmp/img_{nb}_colormap.png', cv2.cvtColor(colormap, cv2.COLOR_RGB2BGR).astype(np.uint8))
            cv2.imwrite(f'tmp/img_{nb}_heightmap.png', heightmap.astype(np.uint8))
            cv2.imwrite(f'tmp/img_{nb}_bead_mask.png', bead_mask)
            cv2.imwrite(f'tmp/img_{nb}_place_rgb.png', cv2.cvtColor(place_rgb, cv2.COLOR_RGB2BGR))
            cv2.imwrite(f'tmp/img_{nb}_place_pixels_eroded.png', np.uint8(place_pixels_eroded * 255))
            if self.task_stage == 2 and len(self.items_in_bag_IDs) > 0:
                pixels_no_bag_items *= 255
                cv2.imwrite(f'tmp/img_{nb}_pixels_no_bag_items.png', np.uint8(pixels_no_bag_items))
        if self.task_stage == 1:
            if np.sum(place_pixels_eroded) > 0:
                self.task_stage = 2
        return (True, place_pixels_eroded)
def determine_task_stage ( self , colormap = None , heightmap = None , object_mask = None , visible_beads = None ) : if self . task_stage == 2 and ( len ( self . items_in_bag_IDs ) == len ( self . item_IDs ) ) : self . task_stage = 3 return ( True , None ) elif self . task_stage == 3 : return ( True , None ) BUF = 0.025 if self . task_stage == 2 : BUF = 0.015 cable_IDs = np . array ( self . cable_bead_IDs ) bead_mask = np . isin ( object_mask , test_elements = cable_IDs ) bead_mask = np . uint8 ( bead_mask * 255 ) contours , _ = cv2 . findContours ( bead_mask , cv2 . RETR_TREE , cv2 . CHAIN_APPROX_SIMPLE ) frac_visible = len ( visible_beads ) / len ( self . cable_bead_IDs ) if len ( contours ) == 0 or frac_visible <= BEAD_THRESH : return ( False , None ) allc = np . concatenate ( [ c for c in contours ] ) contours_list = [ allc ] hull_list = [ cv2 . convexHull ( c ) for c in contours_list ] hull = np . zeros ( ( bead_mask . shape [ 0 ] , bead_mask . shape [ 1 ] , 3 ) , dtype = np . uint8 ) cv2 . drawContours ( hull , hull_list , - 1 , ( 255 , 255 , 255 ) , thickness = - 1 ) hull = cv2 . cvtColor ( hull , cv2 . COLOR_BGR2GRAY ) object_size = self . item_sizes [ 0 ] object_size = ( object_size [ 0 ] + BUF , object_size [ 1 ] + BUF , object_size [ 2 ] ) max_size = np . sqrt ( object_size [ 0 ] ** 2 + object_size [ 1 ] ** 2 ) erode_size = int ( np . round ( max_size / self . pixel_size ) ) if self . task_stage == 2 and len ( self . items_in_bag_IDs ) > 0 : pixels_bag_items = np . ones ( ( hull . shape [ 0 ] , hull . shape [ 1 ] ) , dtype = np . uint8 ) for item_id in self . items_in_bag_IDs : item_pix = np . uint8 ( item_id == object_mask ) pixels_bag_items = pixels_bag_items & item_pix pixels_no_bag_items = np . uint8 ( 1 - pixels_bag_items ) else : pixels_no_bag_items = np . ones ( ( hull . shape [ 0 ] , hull . shape [ 1 ] ) , dtype = np . uint8 ) place_pixels_hull = np . uint8 ( hull == 255 ) place_pixels = place_pixels_hull & pixels_no_bag_items kernel = np . ones ( ( erode_size , erode_size ) , np . uint8 ) place_pixels_eroded = cv2 . erode ( place_pixels , kernel ) if self . task_stage == 2 and np . sum ( place_pixels_eroded ) == 0 : place_pixels_eroded = cv2 . erode ( place_pixels_hull , kernel ) if self . _debug : heightmap = heightmap / np . max ( heightmap ) * 255 place_rgb = cv2 . cvtColor ( hull . copy ( ) , cv2 . COLOR_GRAY2BGR ) place_rgb [ place_pixels_eroded > 0 ] = 127 print ( f'max_size: {max_size:0.3f}, erode_size: {erode_size}' ) print ( f'number of pixels for placing: {np.sum(place_pixels)}' ) print ( f'number of pixels for placing (after eroding): {np.sum(place_pixels_eroded)}' ) nb = len ( [ x for x in os . listdir ( 'tmp/' ) if 'color' in x and '.png' in x ] ) cv2 . imwrite ( f'tmp/img_{nb}_colormap.png' , cv2 . cvtColor ( colormap , cv2 . COLOR_RGB2BGR ) . astype ( np . uint8 ) ) cv2 . imwrite ( f'tmp/img_{nb}_heightmap.png' , heightmap . astype ( np . uint8 ) ) cv2 . imwrite ( f'tmp/img_{nb}_bead_mask.png' , bead_mask ) cv2 . imwrite ( f'tmp/img_{nb}_place_rgb.png' , cv2 . cvtColor ( place_rgb , cv2 . COLOR_RGB2BGR ) ) cv2 . imwrite ( f'tmp/img_{nb}_place_pixels_eroded.png' , np . uint8 ( place_pixels_eroded * 255 ) ) if self . task_stage == 2 and len ( self . items_in_bag_IDs ) > 0 : pixels_no_bag_items *= 255 cv2 . imwrite ( f'tmp/img_{nb}_pixels_no_bag_items.png' , np . uint8 ( pixels_no_bag_items ) ) if self . task_stage == 1 : if np . sum ( place_pixels_eroded ) > 0 : self . task_stage = 2 return ( True , place_pixels_eroded )
Get the task stage in a consistent manner among different policies.
Get the task stage in a consistent manner among different policies .
"""Get the task stage in a consistent manner among different policies.

        When training an oracle policy, we can determine the training stage,
        which is critical because of this task's particular quirks in
        requiring different action parameters (particularly height of the
        pull) for each stage. One option is to use this method to determine
        the hard-coded task stage for each task. This does depend on the
        learned policy inferring when to switch among task stages?

        For policies, I use simpler methods in their classes. They don't call this.

        Returns: False if the task is almost certainly going to fail.
        """ #print('now on task stage 2 --> 3') # Hand-tuned, if too small the agent won't open the bag enough ... # But we can decrease it if we're on task stage 2 and have to put in more items. # Check object_mask for all IDs that correspond to the cable ring. # Threshold image to get 0s and 255s (255s=bead pixels) and find its contours. # If only a few beads are visible (or no contours detected) exit early. # Combine contours via concatenation (shape=(N,1,2)) and get the convex hull. # Make an RGB image, then draw the filled-in area of all items in `hull_list`. # Following task.random_pose, use object_size to find placing points. Assumes # object sizes are same. We add a buffer since convex hulls inflate area. # TODO(daniel) this is where we should really be using rotation. # For the hard bag-items version, get array of 0s = items in bag (hence, # invalid placing points) and 1s = all other points (could be valid). # Logical AND # Make it all 1s so it's safe to apply logical AND with hull pixels. # Combine the hull and pixel conditions. # Use cv2.erode to find valid place pixels. # If we're in task stage 2 and there's nothing, let's revert back to original. # Keep this debugging code to make it easier to inspect. # gray # If on stage 1, and there exists any possible placing point, go to stage 2. #print('now on task stage 1 --> 2')
self
colormap
heightmap
object_mask
visible_beads