#NEWLINE# def calibRightFT( self ) : #NEWLINE# #INDENT# if ( hasattr ( self . robotStateJointController , 'lastRobotStateMessage' ) and self . robotStateJointController . lastRobotStateMessage ) : #NEWLINE# #INDENT# ft = np . array ( ( self . robotStateJointController . lastRobotStateMessage . force_torque . r_hand_force + self . robotStateJointController . lastRobotStateMessage . force_torque . r_hand_torque ) ) #NEWLINE# ft [ 0 ] = ( ft [ 0 ] * ( - 1.0 ) ) #NEWLINE# ft [ 1 ] = ( ft [ 1 ] * ( - 1.0 ) ) #NEWLINE# ft [ 3 ] = ( ft [ 3 ] * ( - 1.0 ) ) #NEWLINE# ft [ 4 ] = ( ft [ 4 ] * ( - 1.0 ) ) #NEWLINE# handMass = 1.0 #NEWLINE# handCOM = [ 0.0 , 0.1 , 0.0 ] #NEWLINE# rHandFrame = self . robotStateModel . getLinkFrame ( 'r_hand_force_torque' ) #NEWLINE# rHandForce = rHandFrame . GetInverse ( ) . TransformPoint ( [ 0 , 0 , ( ( - 9.8 ) * handMass ) ] ) #NEWLINE# self . ft_right_calib_pts . append ( ft ) #NEWLINE# self . ft_right_calib_des . append ( rHandForce ) #NEWLINE# #UNINDENT# self . recomputeBiases ( )