Go to the documentation of this file.00001
00002
00003 import numpy as np, math
00004 from threading import RLock
00005 import copy
00006
00007 import roslib; roslib.load_manifest('hrl_meka_skin_sensor_darpa_m3')
00008 import rospy
00009 import tf
00010
00011 import hrl_lib.transforms as tr
00012
00013 from hrl_msgs.msg import FloatArray
00014 from hrl_msgs.msg import FloatArrayBare
00015 from hrl_haptic_manipulation_in_clutter_msgs.msg import SkinContact
00016 from geometry_msgs.msg import Point
00017 from geometry_msgs.msg import Vector3
00018
00019 class ForceClient():
00020 def __init__(self):
00021 self.raw_data = None
00022 self.fresh_data = False
00023 self.bias = None
00024 self.lock = RLock()
00025 rospy.Subscriber('/sensor/force', FloatArray, self.force_cb)
00026
00027 def force_cb(self, msg):
00028 with self.lock:
00029 self.raw_data = np.array(msg.data)
00030 self.fresh_data = True
00031
00032 def get_raw_data(self, fresh):
00033 if fresh:
00034 while not self.fresh_data:
00035 rospy.sleep(0.002)
00036 with self.lock:
00037 self.fresh_data = False
00038 d = copy.copy(self.raw_data)
00039 return d
00040
00041 def bias_data(self, n):
00042 rospy.logwarn('started biasing...')
00043 d_list = []
00044 for i in range(n):
00045 d_list.append(self.get_raw_data(fresh = True))
00046 d_arr = np.row_stack(d_list)
00047 mn = np.mean(d_arr, 0)
00048 std = np.std(d_arr, 0)
00049 self.bias = mn
00050 self.std = std
00051 rospy.logwarn('...done')
00052
00053
00054
00055 def get_biased_data(self, fresh, n_std=0):
00056 d = self.get_raw_data(fresh)
00057 d_biased = d - self.bias
00058 idxs = np.where(np.abs(d_biased) < n_std * self.std)[0]
00059 d_biased[idxs] = 0
00060 return d_biased
00061
00062
00063 class FT_to_SkinContact():
00064
00065 def __init__(self, arm):
00066 self.use_right_arm = (arm == 'r')
00067 self.sc_pub = rospy.Publisher('/ft/skin_contact', SkinContact)
00068 self.tf_lstnr = tf.TransformListener()
00069
00070 def publish_skin_contact(self, ft):
00071
00072
00073 ft = -np.matrix(ft).T
00074 force = ft[0:3,:]
00075
00076 f_mag = np.linalg.norm(force)
00077 torque = ft[3:,:]
00078
00079 msg = SkinContact()
00080 if self.use_right_arm:
00081 frm_id = '/handmount_RIGHT'
00082 else:
00083 frm_id = '/handmount_LEFT'
00084
00085 msg.header.frame_id = '/torso_lift_link'
00086 msg.header.stamp = rospy.Time.now()
00087
00088 if f_mag > 2.0:
00089 t, q = self.tf_lstnr.lookupTransform('/torso_lift_link',
00090 frm_id, rospy.Time(0))
00091 t = np.matrix(t).reshape(3,1)
00092 rot = tr.quaternion_to_matrix(q)
00093
00094
00095
00096 pt = np.matrix([0., 0., -0.06]).T
00097
00098
00099 if False:
00100
00101
00102
00103
00104
00105 p_oc = np.cross(force.A1, torque.A1) / (force.T * force)[0,0]
00106 pt = np.matrix(p_oc).T + pt
00107
00108
00109 force = rot * force
00110 n = force / f_mag
00111 pt = rot * pt + t
00112
00113 msg.locations.append(Point(pt[0,0], pt[1,0], pt[2,0]))
00114 msg.forces.append(Vector3(force[0,0], force[1,0], force[2,0]))
00115 msg.normals.append(Vector3(n[0,0], n[1,0], n[2,0]))
00116
00117 msg.pts_x.append(FloatArrayBare([pt[0,0]]))
00118 msg.pts_y.append(FloatArrayBare([pt[1,0]]))
00119 msg.pts_z.append(FloatArrayBare([pt[2,0]]))
00120
00121 if self.use_right_arm:
00122 msg.link_names.append('end_effector_RIGHT')
00123 else:
00124 msg.link_names.append('end_effector_LEFT')
00125
00126 self.sc_pub.publish(msg)
00127
00128
00129 if __name__ == '__main__':
00130 rospy.init_node('FT_sensor_to_skin_contact')
00131 ft_to_sc = FT_to_SkinContact('r')
00132 fc = ForceClient()
00133 fc.bias_data(50)
00134
00135 while not rospy.is_shutdown():
00136 ft = fc.get_biased_data(True, 0)
00137 ft_to_sc.publish_skin_contact(ft)
00138
00139
00140