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)
