00001
00002
00003 import sys
00004 import math, numpy as np
00005
00006 import roslib; roslib.load_manifest('hrl_fabric_based_tactile_sensor')
00007 import rospy
00008 from hrl_msgs.msg import FloatArray
00009
00010 import hrl_lib.util as ut
00011 import hrl_lib.transforms as tr
00012
00013 import hrl_fabric_based_tactile_sensor.adc_publisher_node as apn
00014
00015 from m3skin_ros.msg import RawTaxelArray
00016 from geometry_msgs.msg import Transform
00017
00018 from m3skin_ros.srv import None_TransformArray, None_TransformArrayResponse
00019 from m3skin_ros.srv import None_String
00020
00021
00022 class Tactile_Sleeve():
00023 def __init__(self, arm):
00024 self.arm = arm
00025
00026 self.tar_forearm = None_TransformArrayResponse()
00027 self.setup_forearm_twenty_two_taxels_transforms()
00028
00029 self.tar_upperarm = None_TransformArrayResponse()
00030 self.setup_upperarm_taxels_transforms()
00031
00032 self.tar_gripper_right_link = None_TransformArrayResponse()
00033 self.setup_gripper_right_link_taxels_transforms()
00034
00035 self.tar_gripper_left_link = None_TransformArrayResponse()
00036 self.setup_gripper_left_link_taxels_transforms()
00037
00038 self.tar_gripper_palm = None_TransformArrayResponse()
00039 self.setup_gripper_palm_taxels_transforms()
00040
00041 self.tar_left_pps = None_TransformArrayResponse()
00042 self.setup_pps_left_taxels_transforms()
00043
00044 self.tar_right_pps = None_TransformArrayResponse()
00045 self.setup_pps_right_taxels_transforms()
00046
00047
00048
00049
00050
00051 def setup_forearm_twenty_two_taxels_transforms(self):
00052 self.link_name_forearm = '/%s_forearm_link'%(self.arm)
00053 n_taxels = 22
00054
00055 self.tar_forearm.data = [Transform()] * n_taxels
00056 idx_list = [14, 10, 16, 8, 9, 12, 0, 1, 4, 19, 21, 15, 7, 13, 2, 3, 6, 5, 20, 17, 11, 18]
00057 x_disp = [.16, .23, .3, .16, .23, .3, .16, .23, .3, .16, .23, .3, .17, .28, .17, .28, .17, .28, .17, .28, .34, .34]
00058 y_disp = [0., 0., 0., -0.06, -0.06, -0.06, 0., 0., 0., 0.06, 0.06, 0.06, -0.05, -0.05, -0.05, -0.05, 0.05, 0.05, 0.05, 0.05 ,-0.05, 0.05]
00059 z_disp = [0.04, 0.02, 0.03, 0., 0., 0., -0.05, -0.05, -0.05, 0., 0., 0., 0.04, 0.02, -0.04, -0.04, -0.04, -0.04, 0.04, 0.02, 0., 0.]
00060
00061 x_ang = [0, 0, 0, -math.pi/2, -math.pi/2, -math.pi/2, math.pi, math.pi, math.pi, math.pi/2, math.pi/2, math.pi/2, -math.pi/4, -math.pi/4, -3*math.pi/4, -3*math.pi/4, 3*math.pi/4, 3*math.pi/4, math.pi/4, math.pi/4, math.radians(-30), math.radians(30)]
00062 y_ang = [-math.pi/4, math.radians(-15), math.radians(20), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, math.radians(-60), math.radians(-60)]
00063 z_ang = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
00064
00065 for i in range(n_taxels):
00066 t = Transform()
00067 t.translation.x = x_disp[i]
00068 t.translation.y = y_disp[i]
00069 t.translation.z = z_disp[i]
00070
00071 rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
00072 quat = tr.matrix_to_quaternion(rot_mat)
00073
00074 t.rotation.x = quat[0]
00075 t.rotation.y = quat[1]
00076 t.rotation.z = quat[2]
00077 t.rotation.w = quat[3]
00078 self.tar_forearm.data[idx_list[i]] = t
00079
00080 def setup_upperarm_taxels_transforms(self):
00081 n_circum = 4
00082 n_axis = 1
00083 self.link_name_upperarm = '/%s_upper_arm_link'%(self.arm)
00084
00085 angle_along_circum = 2*math.pi / n_circum
00086 offset_along_circum = math.radians(0)
00087
00088 n_taxels = n_circum * n_axis
00089
00090 self.tar_upperarm.data = [None for i in range(n_taxels)]
00091
00092
00093
00094
00095 idx_list = [3, 1, 2, 0]
00096 x_disp = [.3, .3, .3, 0]
00097 y_disp = [0.06, 0, -0.06, 0]
00098 z_disp = [-0.03, -0.09, -0.03, 0]
00099 x_ang = [0, 0, 3*math.pi/2, 0]
00100 y_ang = [math.pi/2, math.pi, 0, 0]
00101 z_ang = [math.pi/2, 0, 0, 0]
00102
00103 for i in range(n_axis):
00104 for j in range(n_circum):
00105 t = Transform()
00106 t.translation.x = x_disp[j]
00107 t.translation.y = y_disp[j]
00108 t.translation.z = z_disp[j]
00109
00110 rot_mat = tr.Rz(z_ang[j])*tr.Ry(y_ang[j])*tr.Rx(x_ang[j])
00111 quat = tr.matrix_to_quaternion(rot_mat)
00112
00113 t.rotation.x = quat[0]
00114 t.rotation.y = quat[1]
00115 t.rotation.z = quat[2]
00116 t.rotation.w = quat[3]
00117 self.tar_upperarm.data[idx_list[i*n_circum+j]] = t
00118
00119 def setup_gripper_palm_taxels_transforms(self):
00120 self.link_name_gripper_palm = '/%s_gripper_palm_link'%(self.arm)
00121 n_taxels = 2
00122
00123 self.tar_gripper_palm.data = [None for i in range(n_taxels)]
00124 idx_list = [0, 1]
00125 x_disp = [0.06, 0.06]
00126 y_disp = [-0.02, 0.02]
00127 z_disp = [0., 0.]
00128 x_ang = [-math.pi/2, math.pi/2]
00129 y_ang = [0, 0]
00130 z_ang = [math.pi/4, -math.pi/4]
00131
00132 for i in range(n_taxels):
00133 t = Transform()
00134 t.translation.x = x_disp[i]
00135 t.translation.y = y_disp[i]
00136 t.translation.z = z_disp[i]
00137
00138 rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
00139 quat = tr.matrix_to_quaternion(rot_mat)
00140
00141 t.rotation.x = quat[0]
00142 t.rotation.y = quat[1]
00143 t.rotation.z = quat[2]
00144 t.rotation.w = quat[3]
00145 self.tar_gripper_palm.data[idx_list[i]] = t
00146
00147 def setup_gripper_left_link_taxels_transforms(self):
00148 self.link_name_gripper_left_link = '/%s_gripper_l_finger_link'%(self.arm)
00149 n_taxels = 4
00150
00151 self.tar_gripper_left_link.data = [None for i in range(n_taxels)]
00152 idx_list = [0, 1, 2, 3]
00153 x_disp = [.03, .04, .03, 0.1]
00154 y_disp = [0.02, 0.04, 0.02, 0.02]
00155 z_disp = [0.03, 0., -0.03, 0.]
00156 x_ang = [0, math.pi/2, math.pi, math.pi/2]
00157 y_ang = [math.radians(-5), 0, math.radians(5), 0]
00158 z_ang = [0, math.radians(10), 0, math.pi/4]
00159
00160 for i in range(n_taxels):
00161 t = Transform()
00162 t.translation.x = x_disp[i]
00163 t.translation.y = y_disp[i]
00164 t.translation.z = z_disp[i]
00165
00166 rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
00167 quat = tr.matrix_to_quaternion(rot_mat)
00168
00169 t.rotation.x = quat[0]
00170 t.rotation.y = quat[1]
00171 t.rotation.z = quat[2]
00172 t.rotation.w = quat[3]
00173 self.tar_gripper_left_link.data[idx_list[i]] = t
00174
00175 def setup_gripper_right_link_taxels_transforms(self):
00176 self.link_name_gripper_right_link = '/%s_gripper_r_finger_link'%(self.arm)
00177 n_taxels = 4
00178
00179 self.tar_gripper_right_link.data = [None for i in range(n_taxels)]
00180 idx_list = [0, 1, 2, 3]
00181 x_disp = [.03, .04, .03, 0.1]
00182 y_disp = [-0.02, -0.04, -0.02, -0.02]
00183 z_disp = [0.03, 0., -0.03, 0.]
00184 x_ang = [0, -math.pi/2, math.pi, -math.pi/2]
00185 y_ang = [math.radians(-5), 0, math.radians(5), 0]
00186 z_ang = [0, math.radians(-10), 0, -math.pi/4]
00187
00188 for i in range(n_taxels):
00189 t = Transform()
00190 t.translation.x = x_disp[i]
00191 t.translation.y = y_disp[i]
00192 t.translation.z = z_disp[i]
00193
00194 rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
00195 quat = tr.matrix_to_quaternion(rot_mat)
00196
00197 t.rotation.x = quat[0]
00198 t.rotation.y = quat[1]
00199 t.rotation.z = quat[2]
00200 t.rotation.w = quat[3]
00201 self.tar_gripper_right_link.data[idx_list[i]] = t
00202
00203 def setup_pps_right_taxels_transforms(self):
00204 self.link_name_right_pps = '/%s_gripper_r_finger_tip_link'%(self.arm)
00205 n_taxels = 3
00206
00207 self.tar_right_pps.data = [None for i in range(n_taxels)]
00208 idx_list = [0, 1, 2]
00209 x_disp = [0.03, 0.012, 0.012]
00210 y_disp = [0.01, 0.01, 0.01]
00211 z_disp = [0.0, -0.011, 0.011]
00212 x_ang = [0., math.pi, 0.]
00213 y_ang = [-math.pi/2., 0., 0.]
00214 z_ang = [0., 0., 0.]
00215
00216 for i in range(n_taxels):
00217 t = Transform()
00218 t.translation.x = x_disp[i]
00219 t.translation.y = y_disp[i]
00220 t.translation.z = z_disp[i]
00221
00222 rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
00223 quat = tr.matrix_to_quaternion(rot_mat)
00224
00225 t.rotation.x = quat[0]
00226 t.rotation.y = quat[1]
00227 t.rotation.z = quat[2]
00228 t.rotation.w = quat[3]
00229 self.tar_right_pps.data[idx_list[i]] = t
00230
00231 def setup_pps_left_taxels_transforms(self):
00232 self.link_name_left_pps = '/%s_gripper_l_finger_tip_link'%(self.arm)
00233 n_taxels = 3
00234
00235 self.tar_left_pps.data = [None for i in range(n_taxels)]
00236 idx_list = [0, 1, 2]
00237 x_disp = [0.03, 0.012, 0.012]
00238 y_disp = [-0.01, -0.01, -0.01]
00239 z_disp = [0.0, -0.011, 0.011]
00240 x_ang = [0., math.pi, 0.]
00241 y_ang = [-math.pi/2., 0., 0.]
00242 z_ang = [0., 0., 0.]
00243
00244 for i in range(n_taxels):
00245 t = Transform()
00246 t.translation.x = x_disp[i]
00247 t.translation.y = y_disp[i]
00248 t.translation.z = z_disp[i]
00249
00250 rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
00251 quat = tr.matrix_to_quaternion(rot_mat)
00252
00253 t.rotation.x = quat[0]
00254 t.rotation.y = quat[1]
00255 t.rotation.z = quat[2]
00256 t.rotation.w = quat[3]
00257 self.tar_left_pps.data[idx_list[i]] = t
00258
00259 def local_coord_frames_forearm_cb(self, req):
00260 return self.tar_forearm
00261
00262 def local_coord_frames_upperarm_cb(self, req):
00263 return self.tar_upperarm
00264
00265 def local_coord_frames_gripper_cb(self, req):
00266 return self.tar_gripper
00267
00268 def local_coord_frames_gripper_right_link_cb(self, req):
00269 return self.tar_gripper_right_link
00270
00271 def local_coord_frames_gripper_left_link_cb(self, req):
00272 return self.tar_gripper_left_link
00273
00274 def local_coord_frames_gripper_palm_cb(self, req):
00275 return self.tar_gripper_palm
00276
00277 def local_coord_frames_pps_left_cb(self, req):
00278 return self.tar_left_pps
00279
00280 def local_coord_frames_pps_right_cb(self, req):
00281 return self.tar_right_pps
00282
00283 def local_coord_frames_pps_cb(self, req):
00284 return self.tar_pps
00285
00286 def link_name_forearm_cb(self, req):
00287 return self.link_name_forearm
00288
00289 def link_name_upperarm_cb(self, req):
00290 return self.link_name_upperarm
00291
00292 def link_name_gripper_cb(self, req):
00293 return self.link_name_gripper
00294
00295 def link_name_gripper_right_link_cb(self, req):
00296 return self.link_name_gripper_right_link
00297
00298 def link_name_gripper_left_link_cb(self, req):
00299 return self.link_name_gripper_left_link
00300
00301 def link_name_gripper_palm_cb(self, req):
00302 return self.link_name_gripper_palm
00303
00304 def link_name_pps_cb(self, req):
00305 return self.link_name_pps
00306
00307 def link_name_pps_left_cb(self, req):
00308 return self.link_name_left_pps
00309
00310 def link_name_pps_right_cb(self, req):
00311 return self.link_name_right_pps
00312
00313
00314 if __name__ == '__main__':
00315 import optparse
00316 p = optparse.OptionParser()
00317
00318 p.add_option('--serial_dev', action='store',
00319 dest='serial_dev_name', type='string',
00320 help='path to the arduino serial device')
00321 p.add_option('--forearm', action='store_true',
00322 dest='forearm',
00323 help='node for the forearm taxels of the sleeve')
00324 p.add_option('--upperarm', action='store_true',
00325 dest='upperarm',
00326 help='node for the upperarm taxels of the sleeve')
00327 p.add_option('--arm_to_use', action='store',
00328 dest='arm', type='string',
00329 help='l or r')
00330
00331 opt, args = p.parse_args()
00332
00333 raw_data_forearm_pub = rospy.Publisher('taxels/raw_data',
00334 RawTaxelArray)
00335
00336 if opt.arm != 'r' and opt.arm != 'l':
00337 rospy.logerr('specify valid arm_to_use (l or r)')
00338 sys.exit()
00339
00340 ts = Tactile_Sleeve(opt.arm)
00341
00342 if opt.forearm:
00343 rospy.Service('taxels/srv/local_coord_frames',
00344 None_TransformArray, ts.local_coord_frames_forearm_cb)
00345 rospy.Service('taxels/srv/link_name', None_String,
00346 ts.link_name_forearm_cb)
00347 n_taxels = 4
00348 elif opt.upperarm:
00349 rospy.Service('taxels/srv/local_coord_frames',
00350 None_TransformArray, ts.local_coord_frames_upperarm_cb)
00351 rospy.Service('taxels/srv/link_name', None_String, ts.link_name_upperarm_cb)
00352 n_taxels = 4
00353 else:
00354 rospy.logerr('Specify either --forearm or --upperarm')
00355 sys.exit()
00356
00357 rospy.init_node('fabric_tactile_sleeve_driver_node')
00358
00359 baudrate = 115200
00360 dev = apn.setup_serial(opt.serial_dev_name, baudrate)
00361
00362 for i in range(10):
00363 dev.readline()
00364
00365 rospy.loginfo('Started publishing data')
00366
00367 rta = RawTaxelArray()
00368 while not rospy.is_shutdown():
00369 rta.val_z = apn.get_adc_data(dev, 16)[0:n_taxels]
00370 raw_data_forearm_pub.publish(rta)
00371
00372 dev.close()
00373
00374