ravens/gripper.py
def detect_contact(self, def_IDs):
        """
        If suction off, detect contact between gripper and objects.
        If suction on, detect contact between picked object and other objects.

        :def_IDs: a list of IDs of deformable objects. Since it may be
        computationally heavy to check, if we have any contact points with
        rigid we just return that right away (w/out checking deformables).
        """
        body, link = self.body, 0
        if self.activated and self.contact_constraint is not None:
            try:
                info = p.getConstraintInfo(self.contact_constraint)
                body, link = info[2], info[3]
            except:
                self.contact_constraint = None
                pass

        # Get all contact points between suction and a rigid body.
        points = p.getContactPoints(bodyA=body, linkIndexA=link)
        if self.activated:
            points = [point for point in points if point[2] != self.body]

        # Normally we return if len(points) > 0, but now if len == 0, we might be
        # missing (a) gripping a deformable or (b) rigid item hitting a deformable.
        if len(points) > 0:
            return True

        # If suction off and len(points)==0, check contact w/deformables. Note:
        # it is critical that we set `self.def_grip_item` correctly.
        if not self.activated:
            gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
            for ID in def_IDs:
                if self.detect_contact_def(gripper_position, ID):
                    self.def_grip_item = ID
                    return True
            return False

        # Here, we're either (a) gripping a deformable, or (b) gripping a rigid
        # item that is NOT touching another rigid item.
        assert self.activated

        if (self.init_grip_item is not None):
            # If suction on, check if a gripped RIGID item touches a deformable.
            # For this, tests were applying if the gripper went through the
            # rigid item, but perhaps also if the fraction is too high?
            # [Haven't seen this in any test scenario, though.]
            object_pose = p.getBasePositionAndOrientation(self.init_grip_item)
            gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
            d = gripper_position - np.float32(object_pose[0])
            distance = np.linalg.norm(d)
            fraction = distance / self.init_grip_distance
            if (fraction <= self.def_frac_lower) or (fraction >= self.def_frac_upper):
                #print(f'[gripping rigid], distance: {distance:0.5f} vs original: '
                #    f'{self.init_grip_distance:0.5f}, and fraction: {fraction:0.5f}')
                # Will release this item, so we don't need these until the next suction.
                self.init_grip_distance = None
                self.init_grip_item = None
            return (fraction <= self.def_frac_lower) or (fraction >= self.def_frac_upper)

        elif (self.def_grip_item is not None):
            # Should see if I ever encounter this in practice? With cloth-cover
            # and cloth-flat I don't think we trigger this condition. UPDATE:
            # ah, upon further tests, that's because the len(points)>0 trigger
            # kicks in when we grip a deformable, but where the suction makes
            # contact. If that happens I'm fine, reduces complexity.
            #
            # TODO: actually need to fix, I think we need suction gripper and
            # the robot's joint just before that. This will not get activated.
            return False
        else:
            # I don't think this should ever invoke -- we should always be
            # gripping a rigid or deformable in this condition.
            return False

        If suction off, detect contact between gripper and objects.
        If suction on, detect contact between picked object and other objects.

        :def_IDs: a list of IDs of deformable objects. Since it may be
        computationally heavy to check, if we have any contact points with
        rigid we just return that right away (w/out checking deformables).
        
If suction off, detect contact between gripper and objects.
If suction on, detect contact between picked object and other objects.
If suction off detect contact between gripper and objects . If suction on detect contact between picked object and other objects .
def detect_contact(self, def_IDs):
        body, link = self.body, 0
        if self.activated and self.contact_constraint is not None:
            try:
                info = p.getConstraintInfo(self.contact_constraint)
                body, link = info[2], info[3]
            except:
                self.contact_constraint = None
                pass
        points = p.getContactPoints(bodyA=body, linkIndexA=link)
        if self.activated:
            points = [point for point in points if point[2] != self.body]
        if len(points) > 0:
            return True
        if not self.activated:
            gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
            for ID in def_IDs:
                if self.detect_contact_def(gripper_position, ID):
                    self.def_grip_item = ID
                    return True
            return False
        assert self.activated
        if (self.init_grip_item is not None):
            object_pose = p.getBasePositionAndOrientation(self.init_grip_item)
            gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
            d = gripper_position - np.float32(object_pose[0])
            distance = np.linalg.norm(d)
            fraction = distance / self.init_grip_distance
            if (fraction <= self.def_frac_lower) or (fraction >= self.def_frac_upper):
                self.init_grip_distance = None
                self.init_grip_item = None
            return (fraction <= self.def_frac_lower) or (fraction >= self.def_frac_upper)
        elif (self.def_grip_item is not None):
             TODO: actually need to fix, I think we need suction gripper and
             the robot's joint just before that. This will not get activated.
            return False
        else:
             I don't think this should ever invoke -- we should always be
             gripping a rigid or deformable in this condition.
            return False
def detect_contact ( self , def_IDs ) : body , link = self . body , 0 if self . activated and self . contact_constraint is not None : try : info = p . getConstraintInfo ( self . contact_constraint ) body , link = info [ 2 ] , info [ 3 ] except : self . contact_constraint = None pass points = p . getContactPoints ( bodyA = body , linkIndexA = link ) if self . activated : points = [ point for point in points if point [ 2 ] != self . body ] if len ( points ) > 0 : return True if not self . activated : gripper_position = np . float32 ( p . getLinkState ( self . body , 0 ) [ 0 ] ) for ID in def_IDs : if self . detect_contact_def ( gripper_position , ID ) : self . def_grip_item = ID return True return False assert self . activated if ( self . init_grip_item is not None ) : object_pose = p . getBasePositionAndOrientation ( self . init_grip_item ) gripper_position = np . float32 ( p . getLinkState ( self . body , 0 ) [ 0 ] ) d = gripper_position - np . float32 ( object_pose [ 0 ] ) distance = np . linalg . norm ( d ) fraction = distance / self . init_grip_distance if ( fraction <= self . def_frac_lower ) or ( fraction >= self . def_frac_upper ) : self . init_grip_distance = None self . init_grip_item = None return ( fraction <= self . def_frac_lower ) or ( fraction >= self . def_frac_upper ) elif ( self . def_grip_item is not None ) : return False else : return False
If suction off, detect contact between gripper and objects.
If suction off detect contact between gripper and objects .
"""
        If suction off, detect contact between gripper and objects.
        If suction on, detect contact between picked object and other objects.

        :def_IDs: a list of IDs of deformable objects. Since it may be
        computationally heavy to check, if we have any contact points with
        rigid we just return that right away (w/out checking deformables).
        """ # Get all contact points between suction and a rigid body. # Normally we return if len(points) > 0, but now if len == 0, we might be # missing (a) gripping a deformable or (b) rigid item hitting a deformable. # If suction off and len(points)==0, check contact w/deformables. Note: # it is critical that we set `self.def_grip_item` correctly. # Here, we're either (a) gripping a deformable, or (b) gripping a rigid # item that is NOT touching another rigid item. # If suction on, check if a gripped RIGID item touches a deformable. # For this, tests were applying if the gripper went through the # rigid item, but perhaps also if the fraction is too high? # [Haven't seen this in any test scenario, though.] #print(f'[gripping rigid], distance: {distance:0.5f} vs original: ' #    f'{self.init_grip_distance:0.5f}, and fraction: {fraction:0.5f}') # Will release this item, so we don't need these until the next suction. # Should see if I ever encounter this in practice? With cloth-cover # and cloth-flat I don't think we trigger this condition. UPDATE: # ah, upon further tests, that's because the len(points)>0 trigger # kicks in when we grip a deformable, but where the suction makes # contact. If that happens I'm fine, reduces complexity. # # TODO: actually need to fix, I think we need suction gripper and # the robot's joint just before that. This will not get activated. # I don't think this should ever invoke -- we should always be # gripping a rigid or deformable in this condition.
self
def_IDs
def_IDs
a list of IDs of deformable objects. Since it may be
computationally heavy to check, if we have any contact points with
rigid we just return that right away (w/out checking deformables).
a list of IDs of deformable objects . Since it may be computationally heavy to check if we have any contact points with rigid we just return that right away ( w / out checking deformables ) .