00001
00002
00003 import roslib
00004 roslib.load_manifest('pr2_controllers_msgs')
00005 roslib.load_manifest('tf')
00006 roslib.load_manifest('rospy')
00007 roslib.load_manifest('geometry_msgs')
00008 roslib.load_manifest('hrl_lib')
00009 import rospy, optparse, math, time
00010 import numpy as np
00011 import tf
00012 import tf.transformations as tr
00013 import cPickle as pkl
00014 import hrl_lib.transforms as hrl_tr
00015 import hrl_lib.util as ut
00016
00017 from tf.msg import tfMessage
00018 from geometry_msgs.msg import TransformStamped, WrenchStamped
00019 from std_msgs.msg import Bool
00020 from pr2_controllers_msgs.msg import JointTrajectoryControllerState
00021
00022
00023 def log_parse():
00024 parser = optparse.OptionParser('Input the Pose node name and the ft sensor node name')
00025
00026 parser.add_option("-t", "--tracker", action="store", type="string",\
00027 dest="tracker_name", default="adl2")
00028 parser.add_option("-f", "--force" , action="store", type="string",\
00029 dest="ft_sensor_name",default="/netft_gravity_zeroing")
00030
00031 (options, args) = parser.parse_args()
00032
00033 return options.tracker_name, options.ft_sensor_name
00034
00035
00036 class PR2_tool_pose():
00037 def __init__(self,tool_frame,tflistener):
00038 self.tool_frame = tool_frame
00039 self.tflistener = tflistener
00040 self.arm_controller_state_name = '/l_arm_controller/state'
00041 self.pr2_jt_sub = rospy.Subscriber(self.arm_controller_state_name,\
00042 JointTrajectoryControllerState, self.pr2_jt_cb )
00043
00044 self.time = 0.
00045 self.init_time = 0.
00046 self.counter = 0
00047 self.counter_prev= 0
00048 self.pos = np.matrix([0.,0.,0.]).T
00049 self.init_pos = np.matrix([0.,0.,0.]).T
00050 self.rot = np.matrix([0.,0.,0.]).T
00051 self.init_rot = np.matrix([0.,0.,0.]).T
00052 self.quat = [0.,0.,0.,0.]
00053 self.jt_name = 0.
00054 self.jt_pos = 0.
00055 self.jt_vel = 0.
00056 self.jt_time = 0.
00057
00058 self.time_data = []
00059 self.pos_data = []
00060 self.rot_data = []
00061 self.quat_data = []
00062 self.jt_name_data = []
00063 self.jt_pos_data = []
00064 self.jt_vel_data = []
00065 self.jt_time_data = []
00066
00067 def set_origin(self):
00068 print 'Set origin of position:', self.tool_frame
00069 print '!!!!!!!!!!!!!!!!!!!!'
00070 b_list = []
00071 r_list = []
00072 for i in range(5):
00073 self.read_tf_pose()
00074 b_list.append(self.pos)
00075 r_list.append(self.rot)
00076 rospy.sleep(1/10.)
00077 if b_list[0] != None:
00078 self.init_pos = np.mean(np.column_stack(b_list),1)
00079 if r_list[0] != None:
00080 self.init_rot = np.mean(np.column_stack(r_list),1)
00081 print 'Origin of position: ', self.init_pos
00082 print 'initial orientation: ', math.degrees(self.init_rot[0,0]),\
00083 math.degrees(self.init_rot[1,0]),\
00084 math.degrees(self.init_rot[2,0])
00085
00086
00087 def read_tf_pose(self):
00088 p, self.quat = self.tflistener.lookupTransform('/base_link',
00089 '/'+self.tool_frame, rospy.Time(0))
00090 self.time = rospy.get_time()
00091 self.pos = np.matrix(p).T
00092 self.rot = np.matrix(tr.euler_from_quaternion(self.quat)).T
00093
00094 self.counter = self.time
00095
00096 def pr2_jt_cb(self,msg):
00097 self.jt_time = msg.header.stamp.to_time()
00098 self.jt_pos = msg.actual.positions
00099 self.jt_vel = msg.actual.velocities
00100 self.jt_name = msg.joint_names
00101
00102 def log(self, log):
00103 self.read_tf_pose()
00104 self.delta_pos = self.pos - self.init_pos
00105 if self.counter > self.counter_prev:
00106 self.counter_prev= self.counter
00107 time_int = self.time-self.init_time
00108
00109 print >> log, time_int, self.counter,\
00110 self.delta_pos[0,0],self.delta_pos[1,0],\
00111 self.delta_pos[2,0],\
00112 self.rot[0,0],self.rot[1,0],self.rot[2,0]
00113
00114 self.rot_data.append(self.rot)
00115 self.quat_data.append(self.quat)
00116 self.pos_data.append(self.delta_pos)
00117 self.time_data.append(self.time)
00118
00119 self.jt_name_data.append(self.jt_name)
00120 self.jt_pos_data.append(self.jt_pos)
00121 self.jt_vel_data.append(self.jt_vel)
00122 self.jt_time_data.append(self.jt_time)
00123
00124
00125 class tool_ft():
00126 def __init__(self,ft_sensor_node_name):
00127 self.init_time = 0.
00128 self.counter = 0
00129 self.counter_prev = 0
00130 self.force = np.matrix([0.,0.,0.]).T
00131 self.force_raw = np.matrix([0.,0.,0.]).T
00132 self.torque = np.matrix([0.,0.,0.]).T
00133 self.torque_raw = np.matrix([0.,0.,0.]).T
00134 self.torque_bias = np.matrix([0.,0.,0.]).T
00135
00136 self.time_data = []
00137 self.force_data = []
00138 self.force_raw_data = []
00139 self.torque_data = []
00140 self.torque_raw_data = []
00141
00142
00143 self.force_sub = rospy.Subscriber(ft_sensor_node_name+\
00144 '/wrench_zeroed', WrenchStamped, self.force_cb)
00145
00146 self.force_raw_sub = rospy.Subscriber('pr2_netft/wrench_raw',\
00147 WrenchStamped, self.force_raw_cb)
00148 self.force_zero = rospy.Publisher(ft_sensor_node_name+\
00149 '/rezero_wrench', Bool)
00150 rospy.logout('Done subscribing to '+ft_sensor_node_name+\
00151 '/rezero_wrench topic')
00152
00153
00154 def force_cb(self, msg):
00155 self.time = msg.header.stamp.to_time()
00156 self.force = np.matrix([msg.wrench.force.x,
00157 msg.wrench.force.y,
00158 msg.wrench.force.z]).T
00159 self.torque = np.matrix([msg.wrench.torque.x,
00160 msg.wrench.torque.y,
00161 msg.wrench.torque.z]).T
00162 self.counter += 1
00163
00164
00165 def force_raw_cb(self, msg):
00166 self.force_raw = np.matrix([msg.wrench.force.x,
00167 msg.wrench.force.y,
00168 msg.wrench.force.z]).T
00169 self.torque_raw = np.matrix([msg.wrench.torque.x,
00170 msg.wrench.torque.y,
00171 msg.wrench.torque.z]).T
00172
00173
00174 def reset(self):
00175 self.force_zero.publish(Bool(True))
00176
00177
00178 def log(self, log_file):
00179 if self.counter > self.counter_prev:
00180 self.counter_prev = self.counter
00181 time_int = self.time-self.init_time
00182
00183 print >> log_file, time_int, self.counter,\
00184 self.force[0,0],self.force[1,0],\
00185 self.force[2,0],\
00186 self.force_raw[0,0],self.force_raw[1,0],\
00187 self.force_raw[2,0],\
00188 self.torque[0,0],self.torque[1,0],\
00189 self.torque[2,0],\
00190 self.torque_raw[0,0],self.torque_raw[1,0],\
00191 self.torque_raw[2,0]
00192
00193 self.force_data.append(self.force)
00194 self.force_raw_data.append(self.force_raw)
00195 self.torque_data.append(self.torque)
00196 self.torque_raw_data.append(self.torque_raw)
00197 self.time_data.append(self.time)
00198
00199
00200 def static_bias(self):
00201 print '!!!!!!!!!!!!!!!!!!!!'
00202 print 'BIASING FT'
00203 print '!!!!!!!!!!!!!!!!!!!!'
00204 f_list = []
00205 t_list = []
00206 for i in range(20):
00207 f_list.append(self.force)
00208 t_list.append(self.torque)
00209 rospy.sleep(2/100.)
00210 if f_list[0] != None and t_list[0] !=None:
00211 self.force_bias = np.mean(np.column_stack(f_list),1)
00212 self.torque_bias = np.mean(np.column_stack(t_list),1)
00213 print self.gravity
00214 print '!!!!!!!!!!!!!!!!!!!!'
00215 print 'DONE Biasing ft'
00216 print '!!!!!!!!!!!!!!!!!!!!'
00217 else:
00218 print 'Biasing Failed!'
00219
00220
00221
00222 class ADL_PR2_log():
00223 def __init__(self):
00224 self.init_time = 0.
00225 rospy.init_node('ADLs_log', anonymous = True)
00226 self.tflistener = tf.TransformListener()
00227 tool_tracker_name, self.ft_sensor_node_name = log_parse()
00228 rospy.logout('ADLs_log node subscribing..')
00229
00230
00231 def tool_cmd_input(self):
00232 confirm = False
00233 while not confirm:
00234 valid = True
00235 self.sub_name=raw_input("Enter subject's name: ")
00236 num=raw_input("Enter the number for the choice of tool:"+\
00237 "\n1) scratcher\n2) shaver\n3) wipe"+\
00238 "\n4) spoon\n5) tooth brush\n6) comb"+\
00239 "\n7) brush\n0) gripper\n: ")
00240 if num == '1':
00241 self.tool_name = 'scratcher'
00242 self.tool_frame_name = 'scratcher'
00243 elif num == '2':
00244 self.tool_name = 'shaver'
00245 self.tool_frame_name = 'l_gripper_shaver45_frame'
00246 elif num == '3':
00247 self.tool_name = 'wipe_finger'
00248 self.tool_frame_name = 'wipe_finger'
00249 elif num == '4':
00250 self.tool_name = 'spoon'
00251 self.tool_frame_name = 'spoon'
00252 elif num == '5':
00253 self.tool_name = 'tooth_brush'
00254 self.tool_frame_name = 'tooth_brush'
00255 elif num == '6':
00256 self.tool_name = 'comb'
00257 self.tool_frame_name = 'comb'
00258 elif num == '7':
00259 self.tool_name = 'brush'
00260 self.tool_frame_name = 'brush'
00261 elif num == '0':
00262 self.tool_name = 'gripper'
00263 self.tool_frame_name = 'l_gripper_tool_frame'
00264 else:
00265 print '\n!!!!!Invalid choice of tool!!!!!\n'
00266 valid = False
00267
00268 if valid:
00269 num=raw_input("Select body:\n1)PR2\n2) caregiver\n: ")
00270 if num == '1':
00271 self.body = 'PR2'
00272 elif num == '2':
00273 self.body = 'caregiver'
00274 else:
00275 print '\n!!!!!Invalid choice of body!!!!!\n'
00276 valid = False
00277 if valid:
00278 self.trial_name=raw_input("Enter trial's name (e.g. arm1, arm2): ")
00279 self.file_name = self.sub_name+'_'+self.tool_name+'_'+self.body+'_'+self.trial_name
00280 ans=raw_input("Enter y to confirm that log file is: "+\
00281 self.file_name+"\n: ")
00282 if ans == 'y':
00283 confirm = True
00284
00285
00286 def init_log_file(self):
00287 self.tool_cmd_input()
00288 self.tooltip = PR2_tool_pose(self.tool_frame_name,self.tflistener)
00289 self.ft = tool_ft(self.ft_sensor_node_name)
00290 self.ft_log_file = open(self.file_name+'_ft.log','w')
00291 self.tooltip_log_file = open(self.file_name+'_tool.log','w')
00292 self.gen_log_file = open(self.file_name+'_gen.log','w')
00293 self.pkl = open(self.file_name+'.pkl','w')
00294 head_p, head_q = self.tflistener.lookupTransform('/base_link',
00295 '/ellipse_frame', rospy.Time(0))
00296 torso_p, torso_q = self.tflistener.lookupTransform('/base_link',
00297 '/torso_lift_link', rospy.Time(0))
00298
00299 raw_input('press Enter to set origin')
00300 self.tooltip.set_origin()
00301
00302
00303 raw_input('press Enter to begin the test')
00304 self.init_time = rospy.get_time()
00305 self.tooltip.init_time = self.init_time
00306 self.ft.init_time = self.init_time
00307
00308 print >> self.gen_log_file,'Begin_time',self.init_time,\
00309 '\ntooltip_init_pos\n X Y Z\n',\
00310 self.tooltip.init_pos[0,0],\
00311 self.tooltip.init_pos[1,0],\
00312 self.tooltip.init_pos[2,0],\
00313 '\ntooltip_init_rot\n Rotx Roty Rotz\n',\
00314 self.tooltip.init_rot[0,0],\
00315 self.tooltip.init_rot[1,0],\
00316 self.tooltip.init_rot[2,0],\
00317 '\nFx Fy Fz Fx_raw Fy_raw Fz_raw\n',\
00318 self.ft.force[0,0],\
00319 self.ft.force[1,0],\
00320 self.ft.force[2,0],\
00321 self.ft.force_raw [0,0],\
00322 self.ft.force_raw [1,0],\
00323 self.ft.force_raw [2,0],\
00324 '\nTx Ty Tz Tx_raw Ty_raw Tz_raw\n',\
00325 self.ft.torque[0,0],\
00326 self.ft.torque[1,0],\
00327 self.ft.torque[2,0],\
00328 self.ft.torque_raw [0,0],\
00329 self.ft.torque_raw [1,0],\
00330 self.ft.torque_raw [2,0],\
00331 '\nhead_pos\n X Y Z\n',\
00332 head_p[0],head_p[1],head_p[2],\
00333 '\nhead_quat\n Qx Qy Qz Qw\n',\
00334 head_q[0],head_q[1],head_q[2],head_q[3],\
00335 '\ntorso_pos\n X Y Z\n',\
00336 torso_p[0],torso_p[1],torso_p[2],\
00337 '\ntorso_quat\n Qx Qy Qz Qw\n',\
00338 torso_q[0],torso_q[1],torso_q[2],torso_q[3],\
00339
00340
00341 def log_state(self):
00342 self.tooltip.log(self.tooltip_log_file)
00343 self.ft.log(self.ft_log_file)
00344 print '\nTool_Pos\t\tForce',\
00345 '\nX: ', self.tooltip.delta_pos[0,0],'\t',\
00346 self.ft.force[0,0],'\t',\
00347 '\nY: ', self.tooltip.delta_pos[1,0],'\t',\
00348 self.ft.force[1,0],'\t',\
00349 '\nZ: ', self.tooltip.delta_pos[2,0],'\t',\
00350 self.ft.force[2,0],'\t'
00351
00352
00353 def close_log_file(self):
00354 dict = {}
00355 dict['init_time'] = self.init_time
00356 dict['init_pos'] = self.tooltip.init_pos
00357 dict['pos'] = self.tooltip.pos_data
00358 dict['quat'] = self.tooltip.quat_data
00359 dict['rot_data'] = self.tooltip.rot_data
00360 dict['ptime'] = self.tooltip.time_data
00361
00362 dict['jt_name']=self.tooltip.jt_name_data
00363 dict['jt_pos']=self.tooltip.jt_pos_data
00364 dict['jt_vel']=self.tooltip.jt_vel_data
00365 dict['jt_time']=self.tooltip.jt_time_data
00366
00367 dict['force'] = self.ft.force_data
00368 dict['force_raw'] = self.ft.force_raw_data
00369 dict['torque'] = self.ft.torque_data
00370 dict['torque_raw'] = self.ft.torque_raw_data
00371 dict['ftime'] = self.ft.time_data
00372
00373 pkl.dump(dict, self.pkl)
00374 self.pkl.close()
00375
00376 self.ft_log_file.close()
00377 self.tooltip_log_file.close()
00378 self.gen_log_file.close()
00379 print 'Closing.. log files have saved..'
00380
00381
00382 if __name__ == '__main__':
00383 log = ADL_PR2_log()
00384 log.init_log_file()
00385
00386 while not rospy.is_shutdown():
00387 log.log_state()
00388 rospy.sleep(1/1000.)
00389
00390 log.close_log_file()