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