Go to the documentation of this file.00001
00002 import numpy as np
00003 import time
00004
00005 import roslib; roslib.load_manifest('modeling_forces')
00006 import rospy
00007
00008 from std_msgs.msg import Empty
00009 from checkerboard_detector.msg import ObjectDetection
00010
00011 import hrl_lib.util as ut
00012 import hrl_lib.transforms as tr
00013 import force_torque.FTClient as ftc
00014
00015 from opencv.cv import *
00016 from opencv.highgui import *
00017 import cameras.dragonfly as dr
00018
00019 def pose_cb(data, d):
00020 if len(data.objects) != 2:
00021 return
00022
00023 for obj in data.objects:
00024 p = obj.pose.position
00025 pos = np.matrix([p.x, p.y, p.z]).T
00026
00027 q = obj.pose.orientation
00028 rot = tr.quaternion_to_matrix([q.x, q.y, q.z, q.w])
00029 t = data.header.stamp.to_sec()
00030
00031 d[obj.type]['pos'] = pos
00032 d[obj.type]['rot'] = rot
00033
00034 def init_ft_client():
00035 client = ftc.FTClient('/force_torque_ft2')
00036 ft = client.read(fresh=True, with_time_stamp=False)
00037 return client
00038
00039
00040
00041
00042
00043 def get_number(prompt, dtype):
00044 no_number = True
00045 number = -9999.0
00046 while no_number:
00047 print prompt
00048 str = raw_input()
00049 try:
00050 number = dtype(str)
00051 no_number = False
00052 except exceptions.ValueError, e:
00053 print 'That was not a valid %s! Try again.'%(dtype.__str__)
00054
00055 return number
00056
00057
00058 if __name__ == '__main__':
00059
00060 rospy.init_node('mechanism_test_node', anonymous=False)
00061 ft_client = init_ft_client()
00062
00063 current_pose_d = {'mechanism': {}, 'hand': {}}
00064 current_pose_d['mechanism'] = {'pos': None, 'rot': None}
00065 current_pose_d['hand'] = {'pos': None, 'rot': None}
00066
00067 pose_d = {'mechanism': {}, 'hand': {}}
00068 pose_d['mechanism'] = {'pos_list': [], 'rot_list': [], 'time_list': []}
00069 pose_d['hand'] = {'pos_list': [], 'rot_list': [], 'time_list': []}
00070
00071 topic_name = '/checkerdetector/ObjectDetection'
00072
00073 rospy.Subscriber(topic_name, ObjectDetection, pose_cb,
00074 current_pose_d)
00075
00076 camera_name = 'remote_head'
00077 cam = dr.dragonfly2(camera_name)
00078 cam.set_frame_rate(30)
00079 cam.set_auto()
00080 for i in range(10):
00081 im = cam.get_frame()
00082
00083 measured_force_list = []
00084 spring_scale_list = []
00085 time_list = []
00086
00087 i = 0
00088 while True:
00089 print 'hit "a" to take a reading, e to exit'
00090 str = raw_input()
00091 if str == 'a':
00092 ft = ft_client.read(fresh=True, with_time_stamp=False)
00093 measured_force_list.append(ft.A1.tolist())
00094 time_list.append(time.time())
00095 im = cam.get_frame()
00096 im = cam.get_frame()
00097 nm = '%05d.png'%i
00098 cvSaveImage(nm, im)
00099 i += 1
00100
00101
00102
00103
00104
00105 f = get_number('Enter the spring scale reading', dtype = float)
00106 spring_scale_list.append(f)
00107 elif str == 'e':
00108 print 'Exiting ...'
00109 break
00110
00111 ft_dict = {}
00112 ft_dict['ft_list'] = measured_force_list
00113 ft_dict['time_list'] = time_list
00114 fname = 'ft_log_'+ut.formatted_time()+'.pkl'
00115 ut.save_pickle(ft_dict, fname)
00116
00117
00118
00119
00120
00121 ut.save_pickle(spring_scale_list,
00122 'spring_scale_'+ut.formatted_time()+'.pkl')
00123
00124
00125