#NEWLINE# def doFTDraw( self , unusedrobotstate ) : #NEWLINE# #INDENT# frames = [ ] #NEWLINE# fts = [ ] #NEWLINE# vis_names = [ ] #NEWLINE# if ( hasattr ( self . robotStateJointController , 'lastRobotStateMessage' ) and self . robotStateJointController . lastRobotStateMessage ) : #NEWLINE# #INDENT# if self . draw_right : #NEWLINE# #INDENT# rft = np . array ( ( self . robotStateJointController . lastRobotStateMessage . force_torque . r_hand_force + self . robotStateJointController . lastRobotStateMessage . force_torque . r_hand_torque ) ) #NEWLINE# rft [ 0 ] = ( ( - 1.0 ) * rft [ 0 ] ) #NEWLINE# rft [ 1 ] = ( ( - 1.0 ) * rft [ 1 ] ) #NEWLINE# rft [ 3 ] = ( ( - 1.0 ) * rft [ 3 ] ) #NEWLINE# rft [ 4 ] = ( ( - 1.0 ) * rft [ 4 ] ) #NEWLINE# rft -= self . ft_right_bias #NEWLINE# fts . append ( rft ) #NEWLINE# frames . append ( self . robotStateModel . getLinkFrame ( 'r_hand_force_torque' ) ) #NEWLINE# vis_names . append ( 'ft_right' ) #NEWLINE# #UNINDENT# if self . draw_left : #NEWLINE# #INDENT# lft = np . array ( ( self . robotStateJointController . lastRobotStateMessage . force_torque . l_hand_force + self . robotStateJointController . lastRobotStateMessage . force_torque . l_hand_torque ) ) #NEWLINE# lft -= self . ft_left_bias #NEWLINE# fts . append ( lft ) #NEWLINE# frames . append ( self . robotStateModel . getLinkFrame ( 'l_hand_force_torque' ) ) #NEWLINE# vis_names . append ( 'ft_left' ) #NEWLINE# #UNINDENT# #UNINDENT# for i in range ( 0 , len ( frames ) ) : #NEWLINE# #INDENT# frame = frames [ i ] #NEWLINE# ft = fts [ i ] #NEWLINE# offset = [ 0.0 , 0.0 , 0.0 ] #NEWLINE# p1 = frame . TransformPoint ( offset ) #NEWLINE# scale = ( 1.0 / 25.0 ) #NEWLINE# scalet = ( 1.0 / 2.5 ) #NEWLINE# p2f = frame . TransformPoint ( ( offset + ( ft [ 0 : 3 ] * scale ) ) ) #NEWLINE# p2t = frame . TransformPoint ( ( offset + ft [ 3 : 6 ] ) ) #NEWLINE# normalt = ( np . array ( p2t ) - np . array ( p1 ) ) #NEWLINE# normalt = ( normalt / np . linalg . norm ( normalt ) ) #NEWLINE# d = DebugData ( ) #NEWLINE# if ( np . linalg . norm ( ( np . array ( p2f ) - np . array ( p1 ) ) ) < 0.1 ) : #NEWLINE# #INDENT# d . addLine ( p1 , p2f , color = [ 1.0 , 0.0 , 0.0 ] ) #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# d . addArrow ( p1 , p2f , color = [ 1.0 , 0.0 , 0.0 ] ) #NEWLINE# #UNINDENT# if self . draw_torque : #NEWLINE# #INDENT# d . addCircle ( p1 , normalt , ( scalet * np . linalg . norm ( ft [ 3 : 6 ] ) ) ) #NEWLINE# #UNINDENT# vis . updateFrame ( frame , ( vis_names [ i ] + 'frame' ) , view = self . view , parent = 'wristft' , visible = False , scale = 0.2 ) #NEWLINE# vis . updatePolyData ( d . getPolyData ( ) , vis_names [ i ] , view = self . view , parent = 'wristft' )