$search
00001 #!/usr/bin/env python 00002 import roslib; roslib.load_manifest('schunk_simulated_tactile_sensors') 00003 import rospy 00004 import math 00005 from schunk_sdh.msg import * 00006 from gazebo_plugins.msg import * 00007 from geometry_msgs.msg import * 00008 00009 00010 class GazeboTactilePad(): 00011 00012 def __init__(self, topic_name, finger_length, finger_width): 00013 self.finger_length = finger_length 00014 self.finger_width = finger_width 00015 00016 # get parameters from parameter server 00017 self.cells_x = rospy.get_param("~cells_x", 6) 00018 self.cells_y = rospy.get_param("~cells_y", 14) 00019 rospy.logdebug("size of the tactile matrix is %ix%i patches", self.cells_x, self.cells_y) 00020 00021 self.sensitivity = rospy.get_param("~sensitivity", 350.0) 00022 rospy.logdebug("sensitivity: %f", self.sensitivity) 00023 00024 self.range = rospy.get_param("~output_range", 3500) 00025 rospy.logdebug("output range: %f", self.range) 00026 00027 self.filter_length = rospy.get_param("~filter_length", 10) 00028 rospy.logdebug("filter length: %i", self.filter_length) 00029 00030 rospy.Subscriber(topic_name, ContactsState, self.contact_callback) 00031 rospy.logdebug("subscribed to bumper states topic: %s", topic_name) 00032 00033 self.smoothed_matrix = self.create_empty_force_matrix() 00034 00035 00036 ''' 00037 Update the pad with the Gazebo contact information. 00038 00039 :param states: gazebo_plugins.msg.ContactsState 00040 ''' 00041 def contact_callback(self, states): 00042 matrix = self.create_empty_force_matrix() 00043 matrix = self.update(states, matrix) 00044 matrix = self.smooth(matrix) 00045 matrix = self.time_average(self.smoothed_matrix, matrix, self.filter_length) 00046 00047 self.smoothed_matrix = matrix 00048 00049 00050 ''' 00051 Update the provided matrix with the contact information from the Gazebo 00052 simulator. 00053 00054 :param states: gazebo_plugins.msg.ContactsState 00055 :param matrix: Float[] 00056 ''' 00057 def update(self, states, matrix): 00058 # no contact, so we don't update the matrix 00059 if (len(states.states) == 0): 00060 return matrix 00061 00062 for i in range(len(states.states)): 00063 state = states.states[i] 00064 for j in range(len(state.contact_positions)): 00065 # check if the contact is on the same side as the sensor 00066 if (state.contact_positions[j].x < 0.0): 00067 continue 00068 00069 # normalize the contact position along the tactile sensor 00070 # we assume that the tactile sensor occupies the complete 00071 # surface of the inner finger side, so finger size is equal to 00072 # sensor size 00073 normalized_x = (state.contact_positions[j].y / self.finger_width) + 0.5 00074 normalized_y = state.contact_positions[j].z / self.finger_length 00075 00076 # from the normalized coordinate we can now determine the index 00077 # of the tactile patch that is activated by the contact 00078 x = round(normalized_x * self.cells_x) 00079 y = round(normalized_y * self.cells_y) 00080 00081 force = -state.wrenches[j].force.x 00082 if (force < 0.0): force = 0.0 00083 00084 index = int(self.get_index(x, y)) 00085 00086 matrix[index] += force 00087 00088 return matrix 00089 00090 00091 ''' 00092 Create a new matrix that contains the current force on each cell. Initialize 00093 all cells with zeros. 00094 00095 :return: Float[] 00096 ''' 00097 def create_empty_force_matrix(self): 00098 matrix = [] 00099 for i in range(self.cells_x * self.cells_y): 00100 matrix.append(0.0) 00101 00102 return matrix 00103 00104 00105 ''' 00106 Run a moving average on the provided matrix. 00107 00108 :param matrix: Float[] 00109 :return: Float[] 00110 ''' 00111 def smooth(self, matrix): 00112 smoothed_matrix = self.create_empty_force_matrix() 00113 00114 for x in range(0, self.cells_x): 00115 for y in range(0, self.cells_y): 00116 sum = 0.0 00117 count = 0 00118 for dx in range(-1, 2): 00119 for dy in range(-1, 2): 00120 index_x = x + dx 00121 index_y = y + dy 00122 00123 if (index_x < 0 or index_x >= self.cells_x): continue 00124 if (index_y < 0 or index_y >= self.cells_y): continue 00125 00126 index = self.get_index(index_x, index_y) 00127 sum += matrix[index] 00128 count += 1 00129 index = self.get_index(x, y) 00130 smoothed_matrix[index] = sum / count 00131 00132 return smoothed_matrix 00133 00134 00135 ''' 00136 Calculate the average matrix from the force buffer. 00137 00138 :return: Float[] 00139 ''' 00140 def time_average(self, matrix_buffer, current_matrix, filter_length): 00141 matrix = self.create_empty_force_matrix() 00142 sample_factor = 1.0 / filter_length 00143 00144 for i in range(self.cells_x * self.cells_y): 00145 force = (1.0 - sample_factor) * matrix_buffer[i] 00146 force += sample_factor * current_matrix[i] 00147 matrix[i] = force 00148 00149 return matrix 00150 00151 00152 ''' 00153 Get the current forces as tactile matrix. matrix_id is the identifier of the 00154 tactile matrix and determines which pad produced the data. 00155 00156 :param matrix_id: Integer 00157 :return: schunk_sdh.msg.TactileMatrix 00158 ''' 00159 def tactile_matrix(self, matrix_id): 00160 matrix = TactileMatrix() 00161 matrix.matrix_id = matrix_id 00162 matrix.cells_x = self.cells_x 00163 matrix.cells_y = self.cells_y 00164 00165 m = self.smoothed_matrix 00166 00167 for i in range(self.cells_x * self.cells_y): 00168 force = m[i] * self.sensitivity 00169 if (force < 0.0): force = 0.0 00170 if (force > self.range): force = self.range 00171 matrix.tactile_array.append(force) 00172 00173 return matrix 00174 00175 00176 ''' 00177 Map the two-dimensional coordinate of a tactile patch to an index in the 00178 one-dimensional data array. The coordinates are bound to the upper and lower 00179 limits. 00180 00181 :param x: Integer 00182 :param y: Integer 00183 :return: Integer 00184 ''' 00185 def get_index(self, x, y): 00186 y = self.cells_y - y - 1 00187 if (x >= self.cells_x): x = self.cells_x - 1 00188 if (x < 0): x = 0 00189 if (y >= self.cells_y): y = self.cells_y - 1 00190 if (y < 0): y = 0 00191 00192 return y * self.cells_x + x 00193 00194 00195 00196 00197 class GazeboVirtualTactileSensor(): 00198 ''' 00199 Constants that determine which indices the finger parts have in the 00200 schunk_sdh.msg.TactileSensor matrix. 00201 ''' 00202 ID_FINGER_12 = 0 00203 ID_FINGER_13 = 1 00204 ID_THUMB_2 = 2 00205 ID_THUMB_3 = 3 00206 ID_FINGER_22 = 4 00207 ID_FINGER_23 = 5 00208 00209 00210 def __init__(self): 00211 self.pads = [] 00212 self.pads.append(GazeboTactilePad("thumb_2/state", 0.0865, 0.03)) 00213 self.pads.append(GazeboTactilePad("thumb_3/state", 0.0675, 0.03)) 00214 self.pads.append(GazeboTactilePad("finger_12/state", 0.0865, 0.03)) 00215 self.pads.append(GazeboTactilePad("finger_13/state", 0.0675, 0.03)) 00216 self.pads.append(GazeboTactilePad("finger_22/state", 0.0865, 0.03)) 00217 self.pads.append(GazeboTactilePad("finger_23/state", 0.0675, 0.03)) 00218 00219 self.pub = rospy.Publisher("tactile_data", TactileSensor) 00220 rospy.loginfo("'tactile_data' topic advertized") 00221 00222 00223 ''' 00224 Publish the current state of the simulated tactile sensors. 00225 ''' 00226 def publish(self): 00227 msg = TactileSensor() 00228 msg.header.stamp = rospy.Time.now() 00229 for i in range(6): 00230 msg.tactile_matrix.append(self.pads[i].tactile_matrix(i)) 00231 self.pub.publish(msg) 00232 00233 00234 00235 00236 if __name__ == "__main__": 00237 rospy.init_node('tactile_sensors') 00238 rospy.sleep(0.5) 00239 00240 sensor = GazeboVirtualTactileSensor() 00241 00242 while not rospy.is_shutdown(): 00243 sensor.publish() 00244 rospy.sleep(0.05)