src/conntact/assembly_tools.py
def force_cap_check(self, danger_force=[45, 45, 45], danger_transverse_force=[3.5, 3.5, 3.5], warning_force=[25, 25, 25], warning_transverse_force=[2, 2, 2]):
        """Checks whether any forces or torques are dangerously high. There are two levels of response:
            *Elevated levels of force cause this program to pause for 1s. If forces remain high after pause, 
            the system will enter a freewheeling state
            *Dangerously high forces will kill this program immediately to prevent damage.
        :return: (Bool) True if all is safe; False if a warning stop is requested.
        """
        #Calculate acceptable torque from transverse forces
        radius = np.linalg.norm(self.as_array(self.tool_data[self.activeTCP]['transform'].transform.translation))
        #Set a minimum radius to always permit some torque
        radius = max(3, radius)
        rospy.loginfo_once("For TCP " + self.activeTCP + " moment arm is coming out to " + str(radius))
        warning_torque=[warning_transverse_force[a]*radius for a in range(3)]
        danger_torque=[danger_transverse_force[b]*radius for b in range(3)]
        rospy.loginfo_once("So torques are limited to  " + str(warning_torque) + str(danger_torque))

        if(not (self.vectorRegionCompare_symmetrical(self.as_array(self.current_wrench.wrench.force), danger_force)
            and self.vectorRegionCompare_symmetrical(self.as_array(self.current_wrench.wrench.torque), danger_torque))):
                rospy.logerr("*Very* high force/torque detected! " + str(self.current_wrench.wrench))
                rospy.logerr("Killing program.")
                quit() # kills the program. Since the node is required, it kills the ROS application.
                return False
        if(self.vectorRegionCompare_symmetrical(self.as_array(self.current_wrench.wrench.force), warning_force)):
            if(self.vectorRegionCompare_symmetrical(self.as_array(self.current_wrench.wrench.torque), warning_torque)):
                return True
        rospy.logerr("High force/torque detected! " + str(self.current_wrench.wrench))
        if(self.highForceWarning):
            self.highForceWarning = False
            return False
        else:   
            rospy.logerr("Sleeping for 1s to damp oscillations...")
            self.highForceWarning = True
            rospy.sleep(1) #Want the system to stop for a second in hopes that it prevents higher forces/torques. May not be helping.
        return True
Checks whether any forces or torques are dangerously high. There are two levels of response:
            *Elevated levels of force cause this program to pause for 1s. If forces remain high after pause, 
            the system will enter a freewheeling state
            *Dangerously high forces will kill this program immediately to prevent damage.
        :return: (Bool) True if all is safe; False if a warning stop is requested.
        
Checks whether any forces or torques are dangerously high. There are two levels of response:
Elevated levels of force cause this program to pause for 1s. If forces remain high after pause,
the system will enter a freewheeling state
Dangerously high forces will kill this program immediately to prevent damage.
Checks whether any forces or torques are dangerously high . There are two levels of response : Elevated levels of force cause this program to pause for 1s . If forces remain high after pause the system will enter a freewheeling state Dangerously high forces will kill this program immediately to prevent damage .
def force_cap_check(self, danger_force=[45, 45, 45], danger_transverse_force=[3.5, 3.5, 3.5], warning_force=[25, 25, 25], warning_transverse_force=[2, 2, 2]):
        radius = np.linalg.norm(self.as_array(self.tool_data[self.activeTCP]['transform'].transform.translation))
        radius = max(3, radius)
        rospy.loginfo_once("For TCP " + self.activeTCP + " moment arm is coming out to " + str(radius))
        warning_torque=[warning_transverse_force[a]*radius for a in range(3)]
        danger_torque=[danger_transverse_force[b]*radius for b in range(3)]
        rospy.loginfo_once("So torques are limited to  " + str(warning_torque) + str(danger_torque))
        if(not (self.vectorRegionCompare_symmetrical(self.as_array(self.current_wrench.wrench.force), danger_force)
            and self.vectorRegionCompare_symmetrical(self.as_array(self.current_wrench.wrench.torque), danger_torque))):
                rospy.logerr("*Very* high force/torque detected! " + str(self.current_wrench.wrench))
                rospy.logerr("Killing program.")
                quit() 
                return False
        if(self.vectorRegionCompare_symmetrical(self.as_array(self.current_wrench.wrench.force), warning_force)):
            if(self.vectorRegionCompare_symmetrical(self.as_array(self.current_wrench.wrench.torque), warning_torque)):
                return True
        rospy.logerr("High force/torque detected! " + str(self.current_wrench.wrench))
        if(self.highForceWarning):
            self.highForceWarning = False
            return False
        else:   
            rospy.logerr("Sleeping for 1s to damp oscillations...")
            self.highForceWarning = True
            rospy.sleep(1) 
        return True
def force_cap_check ( self , danger_force = [ 45 , 45 , 45 ] , danger_transverse_force = [ 3.5 , 3.5 , 3.5 ] , warning_force = [ 25 , 25 , 25 ] , warning_transverse_force = [ 2 , 2 , 2 ] ) : radius = np . linalg . norm ( self . as_array ( self . tool_data [ self . activeTCP ] [ 'transform' ] . transform . translation ) ) radius = max ( 3 , radius ) rospy . loginfo_once ( "For TCP " + self . activeTCP + " moment arm is coming out to " + str ( radius ) ) warning_torque = [ warning_transverse_force [ a ] * radius for a in range ( 3 ) ] danger_torque = [ danger_transverse_force [ b ] * radius for b in range ( 3 ) ] rospy . loginfo_once ( "So torques are limited to  " + str ( warning_torque ) + str ( danger_torque ) ) if ( not ( self . vectorRegionCompare_symmetrical ( self . as_array ( self . current_wrench . wrench . force ) , danger_force ) and self . vectorRegionCompare_symmetrical ( self . as_array ( self . current_wrench . wrench . torque ) , danger_torque ) ) ) : rospy . logerr ( "*Very* high force/torque detected! " + str ( self . current_wrench . wrench ) ) rospy . logerr ( "Killing program." ) quit ( ) return False if ( self . vectorRegionCompare_symmetrical ( self . as_array ( self . current_wrench . wrench . force ) , warning_force ) ) : if ( self . vectorRegionCompare_symmetrical ( self . as_array ( self . current_wrench . wrench . torque ) , warning_torque ) ) : return True rospy . logerr ( "High force/torque detected! " + str ( self . current_wrench . wrench ) ) if ( self . highForceWarning ) : self . highForceWarning = False return False else : rospy . logerr ( "Sleeping for 1s to damp oscillations..." ) self . highForceWarning = True rospy . sleep ( 1 ) return True
Checks whether any forces or torques are dangerously high.
Checks whether any forces or torques are dangerously high .
"""Checks whether any forces or torques are dangerously high. There are two levels of response:
            *Elevated levels of force cause this program to pause for 1s. If forces remain high after pause, 
            the system will enter a freewheeling state
            *Dangerously high forces will kill this program immediately to prevent damage.
        :return: (Bool) True if all is safe; False if a warning stop is requested.
        """ #Calculate acceptable torque from transverse forces #Set a minimum radius to always permit some torque # kills the program. Since the node is required, it kills the ROS application. #Want the system to stop for a second in hopes that it prevents higher forces/torques. May not be helping.
(Bool) True if all is safe; False if a warning stop is requested.
( Bool ) True if all is safe ; False if a warning stop is requested .
self
danger_force
danger_transverse_force
warning_force
warning_transverse_force