#NEWLINE# def update( self , unused ) : #NEWLINE# #INDENT# if ( om . findObjectByName ( 'measured cop' ) . getProperty ( 'Visible' ) and self . robotStateJointController . lastRobotStateMessage ) : #NEWLINE# #INDENT# if ( self . dialogVisible == False ) : #NEWLINE# #INDENT# self . warningButton . setVisible ( True ) #NEWLINE# app . getMainWindow ( ) . statusBar ( ) . insertPermanentWidget ( 0 , self . warningButton ) #NEWLINE# self . dialogVisible = True #NEWLINE# #UNINDENT# self . leftInContact = ( self . robotStateJointController . lastRobotStateMessage . force_torque . l_foot_force_z > 500 ) #NEWLINE# self . rightInContact = ( self . robotStateJointController . lastRobotStateMessage . force_torque . r_foot_force_z > 500 ) #NEWLINE# if ( self . rightInContact or self . leftInContact ) : #NEWLINE# #INDENT# lFootFt = [ self . robotStateJointController . lastRobotStateMessage . force_torque . l_foot_torque_x , self . robotStateJointController . lastRobotStateMessage . force_torque . l_foot_torque_y , 0.0 , 0.0 , 0.0 , self . robotStateJointController . lastRobotStateMessage . force_torque . l_foot_force_z ] #NEWLINE# rFootFt = [ self . robotStateJointController . lastRobotStateMessage . force_torque . r_foot_torque_x , self . robotStateJointController . lastRobotStateMessage . force_torque . r_foot_torque_y , 0.0 , 0.0 , 0.0 , self . robotStateJointController . lastRobotStateMessage . force_torque . r_foot_force_z ] #NEWLINE# lFootTransform = self . robotStateModel . getLinkFrame ( self . robotSystem . ikPlanner . leftFootLink ) #NEWLINE# rFootTransform = self . robotStateModel . getLinkFrame ( self . robotSystem . ikPlanner . rightFootLink ) #NEWLINE# rFootOrigin = np . array ( rFootTransform . TransformPoint ( [ 0 , 0 , ( - 0.07645 ) ] ) ) #NEWLINE# lFootOrigin = np . array ( lFootTransform . TransformPoint ( [ 0 , 0 , ( - 0.07645 ) ] ) ) #NEWLINE# measured_cop = self . ddDrakeWrapper . resolveCenterOfPressure ( self . robotStateModel . model , [ self . lFootFtFrameId , self . rFootFtFrameId ] , ( lFootFt + rFootFt ) , [ 0.0 , 0.0 , 1.0 ] , ( ( ( self . rightInContact * rFootOrigin ) + ( self . leftInContact * lFootOrigin ) ) / ( self . leftInContact + self . rightInContact ) ) ) #NEWLINE# allFootContacts = np . empty ( [ 0 , 2 ] ) #NEWLINE# if self . rightInContact : #NEWLINE# #INDENT# rFootContacts = np . array ( [ rFootTransform . TransformPoint ( contact_point ) for contact_point in self . LONG_FOOT_CONTACT_POINTS ] ) #NEWLINE# allFootContacts = np . concatenate ( ( allFootContacts , rFootContacts [ : , 0 : 2 ] ) ) #NEWLINE# #UNINDENT# if self . leftInContact : #NEWLINE# #INDENT# lFootContacts = np . array ( [ lFootTransform . TransformPoint ( contact_point ) for contact_point in self . LONG_FOOT_CONTACT_POINTS ] ) #NEWLINE# allFootContacts = np . concatenate ( ( allFootContacts , lFootContacts [ : , 0 : 2 ] ) ) #NEWLINE# #UNINDENT# num_pts = allFootContacts . shape [ 0 ] #NEWLINE# dist = self . ddDrakeWrapper . drakeSignedDistanceInsideConvexHull ( num_pts , allFootContacts . reshape ( ( num_pts * 2 ) , 1 ) , measured_cop [ 0 : 2 ] ) #NEWLINE# inSafeSupportPolygon = ( dist >= 0 ) #NEWLINE# dist = ( min ( max ( 0 , dist ) , lFootContacts . DESIRED_INTERIOR_DISTANCE ) / self . DESIRED_INTERIOR_DISTANCE ) #NEWLINE# r = int ( ( 255.0 - ( 255.0 * ( dist ** 4 ) ) ) ) #NEWLINE# g = int ( ( 255.0 * ( dist ** 0.25 ) ) ) #NEWLINE# b = 0 #NEWLINE# colorStatus = [ ( r / 255.0 ) , ( g / 255.0 ) , ( b / 255.0 ) ] #NEWLINE# colorStatusString = ( 'rgb(%d, %d, %d)' % ( r , g , b ) ) #NEWLINE# self . warningButton . setStyleSheet ( ( 'background-color:' + colorStatusString ) ) #NEWLINE# d = DebugData ( ) #NEWLINE# d . addSphere ( measured_cop [ 0 : 3 ] , radius = 0.02 ) #NEWLINE# vis . updatePolyData ( d . getPolyData ( ) , 'measured cop' , view = self . view , parent = 'robot state model' ) . setProperty ( 'Color' , colorStatus ) #NEWLINE# #UNINDENT# #UNINDENT# else : #NEWLINE# #INDENT# if self . dialogVisible : #NEWLINE# #INDENT# app . getMainWindow ( ) . statusBar ( ) . removeWidget ( self . warningButton ) #NEWLINE# self . dialogVisible = False