def publish(self): ps = PressureState() ps.header.stamp = rospy.get_rostime(); ps.l_finger_tip = [] ps.r_finger_tip = [] t = rospy.get_time() for i in range(0,22): ph = .1 * t * (i / 22. + 1) ps.l_finger_tip.append(4000*(1+sin(ph))) ps.r_finger_tip.append(4000*(1+cos(ph))) self.pub.publish(ps)
def __init__(self): """ Sets up the subscribtion thread for the pressure listener. May hang if the left gripper is not publishing pressure information. """ self.wait = 0 self.state = "not_touching" self.pressure_threshold = pressure_threshold """ The state of the gripper. Is 'touching' if the gripper is pushing on a table and 'not_touching' if the gripper is not. """ self.avg_pressure = 0 self.status_pub = rospy.Publisher( "l_gripper_force_status", wiping_sensing.msg.GripperForceStatus) self.thread = threading.Thread(target=self.callback, args=(PressureState(), )) self.thread.start() rospy.Subscriber("/pressure/l_gripper_motor", PressureState, self.callback)
def publish(self): self.lock.acquire() # compute fingertip sensor readings from observed contacts sensor_element_count = len(self.sensor_array_info["l"].center) finger_tip_readings = {"l": [0] * sensor_element_count, "r": [0] * sensor_element_count} for tip in ["l", "r"]: for contactnum in range(len(self.contact_positions[tip])): # figure out which sensor array element the contact falls into, if any nearest_array_element = "not found" nearest_array_weighted_dist = 1e6 dists = [] anglediffs = [] weighteddists = [] for arrayelem in range(sensor_element_count): center = self.sensor_array_info[tip].center[arrayelem] halfside1 = self.sensor_array_info[tip].halfside1[arrayelem] halfside2 = self.sensor_array_info[tip].halfside2[arrayelem] normal = self.sensor_array_info[tip].normal[arrayelem] sensedpoint = self.contact_positions[tip][contactnum] sensednormal = self.contact_normals[tip][contactnum] posdiff = self.contact_positions[tip][contactnum] - self.sensor_array_info[tip].center[arrayelem] # distance from sensor plane (magnitude of projection onto normal) dist_from_plane = math.fabs(numpy.dot(posdiff, sensednormal) / vectnorm(sensednormal)) # distance from sensor element rectangle edge (- is inside) halfside1proj = numpy.dot(posdiff, halfside1) / vectnorm(halfside1) halfside2proj = numpy.dot(posdiff, halfside2) / vectnorm(halfside2) halfside1dist = math.fabs(halfside1proj) - vectnorm(halfside1) halfside2dist = math.fabs(halfside2proj) - vectnorm(halfside2) # find the euclidean dist from the sensor element pad if halfside1dist < 0: halfside1dist = 0.0 if halfside2dist < 0: halfside2dist = 0.0 dist = math.sqrt(halfside1dist ** 2 + halfside2dist ** 2 + dist_from_plane ** 2) dists.append(dist) # has to be at least near the element # (within 1 cm; interpenetration can make positions weird) if dist > 0.01 and not DEBUG: continue # angle difference between the two normals normal_angle_diff = anglediff(sensednormal, normal) anglediffs.append(normal_angle_diff) # weight the euclidean distance difference with the normal distance # (1.0 cm = 1 rad; don't want to be on the wrong side, but friction can # make the angles weird) weighted_dist = normal_angle_diff + dist / 0.01 weighteddists.append(weighted_dist) if weighted_dist < nearest_array_weighted_dist: nearest_array_weighted_dist = weighted_dist nearest_array_element = arrayelem if DEBUG: print "tip:", tip print "dists:", pplist(dists) print "anglediffs:", pplist(anglediffs) print "weighteddists:", pplist(weighteddists) # update that sensor element's depth if this depth is greater # (the maximum readings around 10000?) if nearest_array_element != "not found": if DEBUG: print "nearest_array_element:", nearest_array_element print "nearest_array_weighted_dist: %0.3f" % nearest_array_weighted_dist newreading = ( self.contact_depths[tip][contactnum] * self.sensor_array_info[tip].force_per_unit[nearest_array_element] * 5000.0 ) if newreading > 10000: newreading = 10000 if newreading > finger_tip_readings[tip][nearest_array_element]: finger_tip_readings[tip][nearest_array_element] = newreading self.contact_positions[tip] = [] self.contact_normals[tip] = [] self.contact_depths[tip] = [] self.lock.release() if DEBUG: print "left tip readings:", pplist(finger_tip_readings["l"]) print "right tip readings:", pplist(finger_tip_readings["r"]) # fill in the message and publish it ps = PressureState() ps.header.stamp = rospy.get_rostime() ps.l_finger_tip = [] ps.r_finger_tip = [] for i in range(sensor_element_count): ps.l_finger_tip.append(finger_tip_readings["l"][i]) ps.r_finger_tip.append(finger_tip_readings["r"][i]) self.pub.publish(ps)
def publish(self): self.lock.acquire() #compute fingertip sensor readings from observed contacts sensor_element_count = len(self.sensor_array_info['l'].center) finger_tip_readings = { 'l': [0] * sensor_element_count, 'r': [0] * sensor_element_count } for tip in ['l', 'r']: for contactnum in range(len(self.contact_positions[tip])): #figure out which sensor array element the contact falls into, if any nearest_array_element = 'not found' nearest_array_weighted_dist = 1e6 dists = [] anglediffs = [] weighteddists = [] for arrayelem in range(sensor_element_count): center = self.sensor_array_info[tip].center[arrayelem] halfside1 = self.sensor_array_info[tip].halfside1[ arrayelem] halfside2 = self.sensor_array_info[tip].halfside2[ arrayelem] normal = self.sensor_array_info[tip].normal[arrayelem] sensedpoint = self.contact_positions[tip][contactnum] sensednormal = self.contact_normals[tip][contactnum] posdiff = self.contact_positions[tip][contactnum] - \ self.sensor_array_info[tip].center[arrayelem] #distance from sensor plane (magnitude of projection onto normal) dist_from_plane = math.fabs( numpy.dot(posdiff, sensednormal) / vectnorm(sensednormal)) #distance from sensor element rectangle edge (- is inside) halfside1proj = numpy.dot(posdiff, halfside1) / vectnorm(halfside1) halfside2proj = numpy.dot(posdiff, halfside2) / vectnorm(halfside2) halfside1dist = math.fabs(halfside1proj) - vectnorm( halfside1) halfside2dist = math.fabs(halfside2proj) - vectnorm( halfside2) #find the euclidean dist from the sensor element pad if halfside1dist < 0: halfside1dist = 0. if halfside2dist < 0: halfside2dist = 0. dist = math.sqrt(halfside1dist**2 + halfside2dist**2 + dist_from_plane**2) dists.append(dist) #has to be at least near the element #(within 1 cm; interpenetration can make positions weird) if dist > .01 and not DEBUG: continue #angle difference between the two normals normal_angle_diff = anglediff(sensednormal, normal) anglediffs.append(normal_angle_diff) #weight the euclidean distance difference with the normal distance # (1.0 cm = 1 rad; don't want to be on the wrong side, but friction can # make the angles weird) weighted_dist = normal_angle_diff + dist / .01 weighteddists.append(weighted_dist) if weighted_dist < nearest_array_weighted_dist: nearest_array_weighted_dist = weighted_dist nearest_array_element = arrayelem if DEBUG: print "tip:", tip print "dists:", pplist(dists) print "anglediffs:", pplist(anglediffs) print "weighteddists:", pplist(weighteddists) #update that sensor element's depth if this depth is greater #(the maximum readings around 10000?) if nearest_array_element != 'not found': if DEBUG: print "nearest_array_element:", nearest_array_element print "nearest_array_weighted_dist: %0.3f" % nearest_array_weighted_dist newreading = self.contact_depths[tip][contactnum] * \ self.sensor_array_info[tip].force_per_unit[nearest_array_element] * 5000. if newreading > 10000: newreading = 10000 if newreading > finger_tip_readings[tip][ nearest_array_element]: finger_tip_readings[tip][ nearest_array_element] = newreading self.contact_positions[tip] = [] self.contact_normals[tip] = [] self.contact_depths[tip] = [] self.lock.release() if DEBUG: print "left tip readings:", pplist(finger_tip_readings['l']) print "right tip readings:", pplist(finger_tip_readings['r']) #fill in the message and publish it ps = PressureState() ps.header.stamp = rospy.get_rostime() ps.l_finger_tip = [] ps.r_finger_tip = [] for i in range(sensor_element_count): ps.l_finger_tip.append(finger_tip_readings['l'][i]) ps.r_finger_tip.append(finger_tip_readings['r'][i]) self.pub.publish(ps)