ravens/agents/transporter.py
def _determine_task_stage(self, p0_pixel, p1_pixel):
        """Determines task stage for the bag-items tasks.

        Hacky solution, unfortunately, assumes we assigned task.env. Assumes that we
        have an actual `self.real_task` we can use; `self.task` is just a string.
        Currently working reasonably well for bag-items-easy. Note: see gt_state.py
        for the version that works for the gt_state baselines. Also, note that the
        self.real_task.task_stage is reset to 1 whenever we reset() so there's no
        need to deal with that logic here.

        (0) So far this is working reasonably well. Note that:
            - if object_mask[p0_pixel] == 38, then we're picking a cube.
            - for bag-items-hard, ID 39 is also a block (note: no cubes).
            - and if we end up putting both items into the bag, BUT we STILL end up
            with object_mask[p0_pixel] as gripping one of those items, we DO NOT go
            to task stage 3, which makes sense and is correct.

        (1) However, it will not be able to catch this failure:
            - putting two items in the bag.
            - but picking one item out of the bag, and inserting it to the target
            - then properly doing 'task stage 3' by pulling and inserting the bag
            with ONE item into the zone. However, the first item that was pulled out
            may be in the zone, leading to an 'undesirable' success.

        (2) Another potential failure that I observed:
            - FIRST action: sometimes the robot grips an item at the start, which is
            the right thing to do if the bag is already open. But we may grip the block
            EVEN THOUGH the pixel may not correspond to the block in the the segmentation
            mask. This happens when the pick point is just outside the block's pixels.
            Then this will result in a very slow pick and place since we follow task
            stage 1 parameters with slow movements and often get movej time outs if the
            block started far from the bag. In that case we can argue that we should have
            had a better pick point so that we correctly detect that we should move onto
            task stage 2. A better solution would be to check for any contact points
            after we grip, and test for contact with block IDs?
            - I ended up increasing the task stage 1 speed from 0.003 --> 0.004.

        (3) Of course, there is still a failure of picking a vertex instead of a bag bead
        during the bag pulling stage. AH, oh well.
        """
        real_task = self.real_task  # assume we assigned this.
        colormap, heightmap, object_mask = real_task.get_object_masks(real_task.env)

        if False:
            nb = len([x for x in os.listdir('.') if '.png' in x])
            mask = np.array(object_mask / np.max(object_mask) * 255).astype(np.uint8)
            mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)  # debugging
            p0 = (p0_pixel[1], p0_pixel[0])
            p1 = (p1_pixel[1], p1_pixel[0])
            cv2.circle(mask, p0, radius=3, color=(255,0,255), thickness=-1)
            cv2.circle(mask, p1, radius=3, color=(255,255,0), thickness=-1)
            cv2.imwrite(f'mask_{nb}.png', mask)

        # For bag-color-goal, it's fine to reset to task stage 1 if needed.
        if self.task in ['bag-items-easy', 'bag-items-hard']:
            if object_mask[p0_pixel] in [38, 39]:
                real_task.task_stage = 2
            elif real_task.task_stage == 2:
                real_task.task_stage = 3
        elif self.task in ['bag-color-goal']:
            if object_mask[p0_pixel] == real_task.single_block_ID:
                real_task.task_stage = 2
            else:
                real_task.task_stage = 1
        else:
            raise NotImplementedError(self.task)
Determines task stage for the bag-items tasks.

        Hacky solution, unfortunately, assumes we assigned task.env. Assumes that we
        have an actual `self.real_task` we can use; `self.task` is just a string.
        Currently working reasonably well for bag-items-easy. Note: see gt_state.py
        for the version that works for the gt_state baselines. Also, note that the
        self.real_task.task_stage is reset to 1 whenever we reset() so there's no
        need to deal with that logic here.

        (0) So far this is working reasonably well. Note that:
            - if object_mask[p0_pixel] == 38, then we're picking a cube.
            - for bag-items-hard, ID 39 is also a block (note: no cubes).
            - and if we end up putting both items into the bag, BUT we STILL end up
            with object_mask[p0_pixel] as gripping one of those items, we DO NOT go
            to task stage 3, which makes sense and is correct.

        (1) However, it will not be able to catch this failure:
            - putting two items in the bag.
            - but picking one item out of the bag, and inserting it to the target
            - then properly doing 'task stage 3' by pulling and inserting the bag
            with ONE item into the zone. However, the first item that was pulled out
            may be in the zone, leading to an 'undesirable' success.

        (2) Another potential failure that I observed:
            - FIRST action: sometimes the robot grips an item at the start, which is
            the right thing to do if the bag is already open. But we may grip the block
            EVEN THOUGH the pixel may not correspond to the block in the the segmentation
            mask. This happens when the pick point is just outside the block's pixels.
            Then this will result in a very slow pick and place since we follow task
            stage 1 parameters with slow movements and often get movej time outs if the
            block started far from the bag. In that case we can argue that we should have
            had a better pick point so that we correctly detect that we should move onto
            task stage 2. A better solution would be to check for any contact points
            after we grip, and test for contact with block IDs?
            - I ended up increasing the task stage 1 speed from 0.003 --> 0.004.

        (3) Of course, there is still a failure of picking a vertex instead of a bag bead
        during the bag pulling stage. AH, oh well.
        
Determines task stage for the bag-items tasks.
Hacky solution, unfortunately, assumes we assigned task.env. Assumes that we
have an actual `self.real_task` we can use; `self.task` is just a string.
Currently working reasonably well for bag-items-easy. Note: see gt_state.py
for the version that works for the gt_state baselines. Also, note that the
self.real_task.task_stage is reset to 1 whenever we reset() so there's no
need to deal with that logic here.

(0) So far this is working reasonably well. Note that:
if object_mask[p0_pixel] == 38, then we're picking a cube.
for bag-items-hard, ID 39 is also a block (note: no cubes).
and if we end up putting both items into the bag, BUT we STILL end up
with object_mask[p0_pixel] as gripping one of those items, we DO NOT go
to task stage 3, which makes sense and is correct.

(1) However, it will not be able to catch this failure:
putting two items in the bag.
but picking one item out of the bag, and inserting it to the target
then properly doing 'task stage 3' by pulling and inserting the bag
with ONE item into the zone. However, the first item that was pulled out
may be in the zone, leading to an 'undesirable' success.

(2) Another potential failure that I observed:
FIRST action: sometimes the robot grips an item at the start, which is
the right thing to do if the bag is already open. But we may grip the block
EVEN THOUGH the pixel may not correspond to the block in the the segmentation
mask. This happens when the pick point is just outside the block's pixels.
Then this will result in a very slow pick and place since we follow task
stage 1 parameters with slow movements and often get movej time outs if the
block started far from the bag. In that case we can argue that we should have
had a better pick point so that we correctly detect that we should move onto
task stage 2. A better solution would be to check for any contact points
after we grip, and test for contact with block IDs.
I ended up increasing the task stage 1 speed from 0.003 --> 0.004.

(3) Of course, there is still a failure of picking a vertex instead of a bag bead
during the bag pulling stage. AH, oh well.
Determines task stage for the bag - items tasks . Hacky solution unfortunately assumes we assigned task . env . Assumes that we have an actual ` self . real_task ` we can use ; ` self . task ` is just a string . Currently working reasonably well for bag - items - easy . Note : see gt_state . py for the version that works for the gt_state baselines . Also note that the self . real_task . task_stage is reset to 1 whenever we reset () so there ' s no need to deal with that logic here . ( 0 ) So far this is working reasonably well . Note that : if object_mask [ p0_pixel ] == 38 then we ' re picking a cube . for bag - items - hard ID 39 is also a block ( note : no cubes ) . and if we end up putting both items into the bag BUT we STILL end up with object_mask [ p0_pixel ] as gripping one of those items we DO NOT go to task stage 3 which makes sense and is correct . ( 1 ) However it will not be able to catch this failure : putting two items in the bag . but picking one item out of the bag and inserting it to the target then properly doing ' task stage 3 ' by pulling and inserting the bag with ONE item into the zone . However the first item that was pulled out may be in the zone leading to an ' undesirable ' success . ( 2 ) Another potential failure that I observed : FIRST action : sometimes the robot grips an item at the start which is the right thing to do if the bag is already open . But we may grip the block EVEN THOUGH the pixel may not correspond to the block in the the segmentation mask . This happens when the pick point is just outside the block ' s pixels . Then this will result in a very slow pick and place since we follow task stage 1 parameters with slow movements and often get movej time outs if the block started far from the bag . In that case we can argue that we should have had a better pick point so that we correctly detect that we should move onto task stage 2 . A better solution would be to check for any contact points after we grip and test for contact with block IDs . I ended up increasing the task stage 1 speed from 0 . 003 -- > 0 . 004 . ( 3 ) Of course there is still a failure of picking a vertex instead of a bag bead during the bag pulling stage . AH oh well .
def _determine_task_stage(self, p0_pixel, p1_pixel):
        real_task = self.real_task  
        colormap, heightmap, object_mask = real_task.get_object_masks(real_task.env)
        if False:
            nb = len([x for x in os.listdir('.') if '.png' in x])
            mask = np.array(object_mask / np.max(object_mask) * 255).astype(np.uint8)
            mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)  
            p0 = (p0_pixel[1], p0_pixel[0])
            p1 = (p1_pixel[1], p1_pixel[0])
            cv2.circle(mask, p0, radius=3, color=(255,0,255), thickness=-1)
            cv2.circle(mask, p1, radius=3, color=(255,255,0), thickness=-1)
            cv2.imwrite(f'mask_{nb}.png', mask)
        if self.task in ['bag-items-easy', 'bag-items-hard']:
            if object_mask[p0_pixel] in [38, 39]:
                real_task.task_stage = 2
            elif real_task.task_stage == 2:
                real_task.task_stage = 3
        elif self.task in ['bag-color-goal']:
            if object_mask[p0_pixel] == real_task.single_block_ID:
                real_task.task_stage = 2
            else:
                real_task.task_stage = 1
        else:
            raise NotImplementedError(self.task)
def _determine_task_stage ( self , p0_pixel , p1_pixel ) : real_task = self . real_task colormap , heightmap , object_mask = real_task . get_object_masks ( real_task . env ) if False : nb = len ( [ x for x in os . listdir ( '.' ) if '.png' in x ] ) mask = np . array ( object_mask / np . max ( object_mask ) * 255 ) . astype ( np . uint8 ) mask = cv2 . cvtColor ( mask , cv2 . COLOR_GRAY2BGR ) p0 = ( p0_pixel [ 1 ] , p0_pixel [ 0 ] ) p1 = ( p1_pixel [ 1 ] , p1_pixel [ 0 ] ) cv2 . circle ( mask , p0 , radius = 3 , color = ( 255 , 0 , 255 ) , thickness = - 1 ) cv2 . circle ( mask , p1 , radius = 3 , color = ( 255 , 255 , 0 ) , thickness = - 1 ) cv2 . imwrite ( f'mask_{nb}.png' , mask ) if self . task in [ 'bag-items-easy' , 'bag-items-hard' ] : if object_mask [ p0_pixel ] in [ 38 , 39 ] : real_task . task_stage = 2 elif real_task . task_stage == 2 : real_task . task_stage = 3 elif self . task in [ 'bag-color-goal' ] : if object_mask [ p0_pixel ] == real_task . single_block_ID : real_task . task_stage = 2 else : real_task . task_stage = 1 else : raise NotImplementedError ( self . task )
Determines task stage for the bag-items tasks.
Determines task stage for the bag - items tasks .
"""Determines task stage for the bag-items tasks.

        Hacky solution, unfortunately, assumes we assigned task.env. Assumes that we
        have an actual `self.real_task` we can use; `self.task` is just a string.
        Currently working reasonably well for bag-items-easy. Note: see gt_state.py
        for the version that works for the gt_state baselines. Also, note that the
        self.real_task.task_stage is reset to 1 whenever we reset() so there's no
        need to deal with that logic here.

        (0) So far this is working reasonably well. Note that:
            - if object_mask[p0_pixel] == 38, then we're picking a cube.
            - for bag-items-hard, ID 39 is also a block (note: no cubes).
            - and if we end up putting both items into the bag, BUT we STILL end up
            with object_mask[p0_pixel] as gripping one of those items, we DO NOT go
            to task stage 3, which makes sense and is correct.

        (1) However, it will not be able to catch this failure:
            - putting two items in the bag.
            - but picking one item out of the bag, and inserting it to the target
            - then properly doing 'task stage 3' by pulling and inserting the bag
            with ONE item into the zone. However, the first item that was pulled out
            may be in the zone, leading to an 'undesirable' success.

        (2) Another potential failure that I observed:
            - FIRST action: sometimes the robot grips an item at the start, which is
            the right thing to do if the bag is already open. But we may grip the block
            EVEN THOUGH the pixel may not correspond to the block in the the segmentation
            mask. This happens when the pick point is just outside the block's pixels.
            Then this will result in a very slow pick and place since we follow task
            stage 1 parameters with slow movements and often get movej time outs if the
            block started far from the bag. In that case we can argue that we should have
            had a better pick point so that we correctly detect that we should move onto
            task stage 2. A better solution would be to check for any contact points
            after we grip, and test for contact with block IDs?
            - I ended up increasing the task stage 1 speed from 0.003 --> 0.004.

        (3) Of course, there is still a failure of picking a vertex instead of a bag bead
        during the bag pulling stage. AH, oh well.
        """ # assume we assigned this. # debugging # For bag-color-goal, it's fine to reset to task stage 1 if needed.
self
p0_pixel
p1_pixel