pr2_fingertip_pressure_contact_translator.py
Go to the documentation of this file.
1 #! /usr/bin/python
2 
3 #***********************************************************
4 #* Software License Agreement (BSD License)
5 #*
6 #* Copyright (c) 2009, Willow Garage, Inc.
7 #* All rights reserved.
8 #*
9 #* Redistribution and use in source and binary forms, with or without
10 #* modification, are permitted provided that the following conditions
11 #* are met:
12 #*
13 #* * Redistributions of source code must retain the above copyright
14 #* notice, this list of conditions and the following disclaimer.
15 #* * Redistributions in binary form must reproduce the above
16 #* copyright notice, this list of conditions and the following
17 #* disclaimer in the documentation and/or other materials provided
18 #* with the distribution.
19 #* * Neither the name of the Willow Garage nor the names of its
20 #* contributors may be used to endorse or promote products derived
21 #* from this software without specific prior written permission.
22 #*
23 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 #* POSSIBILITY OF SUCH DAMAGE.
35 #***********************************************************
36 
37 # Author: Kaijen Hsiao
38 
39 # Simulates the fingertip sensor array based on the bumper contact states
40 # subscribes to Gazebo's ContactsState messages
41 # publishes PressureState messages
42 
43 import roslib
44 roslib.load_manifest('pr2_gazebo')
45 import rospy
46 import tf
47 from pr2_msgs.msg import PressureState
48 from geometry_msgs.msg import PointStamped, Vector3Stamped
49 from gazebo_msgs.msg import ContactState, ContactsState
50 
51 import numpy
52 import math
53 import threading
54 import pdb
55 import time
56 
57 from fingertip_pressure import fingertip_geometry
58 
59 DEBUG = 0
60 
61 #vector norm of numpy 3-vector (as array)
62 def vectnorm(vect):
63  return math.sqrt(numpy.dot(vect, vect))
64 
65 #vector norm of numpy array of vectors (nx3)
66 def arrayvectnorm(vectarray):
67  try:
68  return [vectnorm(x) for x in vectarray]
69  except:
70  pdb.set_trace()
71 
72 #angle diff of two numpy 3-arrays
73 def anglediff(vect1, vect2):
74  return math.acos(numpy.dot(vect1, vect2)/(vectnorm(vect1)*vectnorm(vect2)))
75 
76 #pretty-print a list of floats to str
77 def pplist(list):
78  return ' '.join(['%2.3f'%x for x in list])
79 
80 #pretty-print a list of lists of floats to str
81 def pplistlist(list):
82  return '\n'.join([pplist(x) for x in list])
83 
84 #convert a Vector3 or a Point msg to a 4x1 numpy matrix
85 def vectorToNumpyMat(vector):
86  return numpy.matrix([[vector.x, vector.y, vector.z, 1]]).transpose()
87 
88 
89 
90 #fingertip array element info
91 #(x toward tip, y away from front in l/toward front in r, z to the left in l/to the right in r)
93  def __init__(self, tip):
94 
95  if tip == 'l':
96  l_msg = fingertip_geometry.pressureInformation('', 1)
97  (self.force_per_unit, self.center, self.halfside1, self.halfside2) = \
98  (l_msg.force_per_unit, l_msg.center, l_msg.halfside1, l_msg.halfside2)
99  else:
100  r_msg = fingertip_geometry.pressureInformation('', -1)
101  (self.force_per_unit, self.center, self.halfside1, self.halfside2) = \
102  (r_msg.force_per_unit, r_msg.center, r_msg.halfside1, r_msg.halfside2)
103 
104  #convert to numpy arrays
105  self.center = [numpy.array([v.x, v.y, v.z]) for v in self.center]
106  self.halfside1 = [numpy.array([v.x, v.y, v.z]) for v in self.halfside1]
107  self.halfside2 = [numpy.array([v.x, v.y, v.z]) for v in self.halfside2]
108 
109  #compute sensor element normals
110  self.normal = numpy.cross(self.halfside1, self.halfside2)
111  self.normal = [self.normal[i] / vectnorm(self.normal[i]) for i in range(len(self.normal))]
112 
113  #print "tip:", tip
114  #print "center:", self.center
115  #print "halfside1:", self.halfside1
116  #print "halfside2:", self.halfside2
117  #print "normal:", self.normal
118 
119 
121 
122  #gripper is 'r' or 'l'
123  def __init__(self, gripper):
124  self.gripper = gripper
125 
126  self.sensor_array_info = {'r': SensorArrayInfo('r'), 'l': SensorArrayInfo('l')}
127 
128  self.lock = threading.Lock()
129 
130  #initialize the dictionaries for the contact positions, normals, and depths
131  self.contact_positions = {'r':[], 'l':[]}
132  self.contact_normals = {'r':[], 'l':[]}
133  self.contact_depths = {'r':[], 'l':[]}
134 
135  #the frames for the fingertip contact array positions
136  self.fingertip_frameid = {'r':gripper+'_gripper_r_finger_tip_link',
137  'l':gripper+'_gripper_l_finger_tip_link'}
138 
139  #subscribe to Gazebo fingertip contact info
140  rospy.Subscriber(gripper + "_gripper_l_finger_tip_bumper/state", ContactsState, self.l_contact_callback)
141  rospy.Subscriber(gripper + "_gripper_r_finger_tip_bumper/state", ContactsState, self.r_contact_callback)
142 
143  #publish contact array info as PressureState messages, just like the real robot does
144  self.pub = rospy.Publisher("pressure/"+gripper+"_gripper_motor", PressureState)
145 
146 
147 
148  #callback for receiving left fingertip ContactsState messages
149  def l_contact_callback(self, bumperstate):
150  self.store_contacts('l', bumperstate)
151 
152  #callback for receiving right fingertip ContactsState messages
153  def r_contact_callback(self, bumperstate):
154  self.store_contacts('r', bumperstate)
155 
156  #store the contact positions and normals in the fingertip frame
157  def store_contacts(self, whichtip, bumperstate):
158 
159  #self.contact_positions[whichtip] = []
160  #self.contact_normals[whichtip] = []
161  #self.contact_depths[whichtip] = []
162 
163  fingertip_frameid = self.fingertip_frameid[whichtip]
164 
165  self.lock.acquire()
166  for contactstate in bumperstate.states:
167 
168  #transform each contact point and normal to the fingertip frame
169  for i in range(len(contactstate.depths)):
170 
171  #account for map offset of (25.7, 25.7, 0)
172  contact_pos = vectorToNumpyMat(contactstate.contact_positions[i])
173  contact_normal = vectorToNumpyMat(contactstate.contact_normals[i])
174  array_pos = numpy.array([contact_pos[0,0], contact_pos[1,0], contact_pos[2,0]])
175  array_normal = numpy.array([contact_normal[0,0], contact_normal[1,0], contact_normal[2,0]])
176  self.contact_positions[whichtip].append(array_pos)
177  self.contact_normals[whichtip].append(array_normal)
178  self.contact_depths[whichtip].append(contactstate.depths[i])
179 
180  self.lock.release()
181 
182  if DEBUG:
183  print whichtip, "positions:\n", pplistlist(self.contact_positions[whichtip])
184  print whichtip, "normals:\n", pplistlist(self.contact_normals[whichtip])
185 
186 
187 
188  #publish the current simulated fingertip sensor array readings
189  def publish(self):
190 
191  self.lock.acquire()
192 
193  #compute fingertip sensor readings from observed contacts
194  sensor_element_count = len(self.sensor_array_info['l'].center)
195  finger_tip_readings = {'l':[0]*sensor_element_count, 'r':[0]*sensor_element_count}
196  for tip in ['l','r']:
197  for contactnum in range(len(self.contact_positions[tip])):
198 
199  #figure out which sensor array element the contact falls into, if any
200  nearest_array_element = 'not found'
201  nearest_array_weighted_dist = 1e6;
202 
203  dists = []
204  anglediffs = []
205  weighteddists = []
206 
207  for arrayelem in range(sensor_element_count):
208 
209  center = self.sensor_array_info[tip].center[arrayelem]
210  halfside1 = self.sensor_array_info[tip].halfside1[arrayelem]
211  halfside2 = self.sensor_array_info[tip].halfside2[arrayelem]
212  normal = self.sensor_array_info[tip].normal[arrayelem]
213 
214  sensedpoint = self.contact_positions[tip][contactnum]
215  sensednormal = self.contact_normals[tip][contactnum]
216 
217  posdiff = self.contact_positions[tip][contactnum] - \
218  self.sensor_array_info[tip].center[arrayelem]
219 
220  #distance from sensor plane (magnitude of projection onto normal)
221  dist_from_plane = math.fabs(numpy.dot(posdiff, sensednormal)/vectnorm(sensednormal))
222 
223  #distance from sensor element rectangle edge (- is inside)
224  halfside1proj = numpy.dot(posdiff, halfside1)/vectnorm(halfside1)
225  halfside2proj = numpy.dot(posdiff, halfside2)/vectnorm(halfside2)
226  halfside1dist = math.fabs(halfside1proj) - vectnorm(halfside1)
227  halfside2dist = math.fabs(halfside2proj) - vectnorm(halfside2)
228 
229  #find the euclidean dist from the sensor element pad
230  if halfside1dist < 0:
231  halfside1dist = 0.
232  if halfside2dist < 0:
233  halfside2dist = 0.
234  dist = math.sqrt(halfside1dist**2+halfside2dist**2+dist_from_plane**2)
235  dists.append(dist)
236 
237  #has to be at least near the element
238  #(within 1 cm; interpenetration can make positions weird)
239  if dist > .01 and not DEBUG:
240  continue
241 
242  #angle difference between the two normals
243  normal_angle_diff = anglediff(sensednormal, normal)
244  anglediffs.append(normal_angle_diff)
245 
246  #weight the euclidean distance difference with the normal distance
247  # (1.0 cm = 1 rad; don't want to be on the wrong side, but friction can
248  # make the angles weird)
249  weighted_dist = normal_angle_diff + dist / .01
250  weighteddists.append(weighted_dist)
251 
252  if weighted_dist < nearest_array_weighted_dist:
253  nearest_array_weighted_dist = weighted_dist
254  nearest_array_element = arrayelem
255 
256  if DEBUG:
257  print "tip:", tip
258  print "dists:", pplist(dists)
259  print "anglediffs:", pplist(anglediffs)
260  print "weighteddists:", pplist(weighteddists)
261 
262  #update that sensor element's depth if this depth is greater
263  #(the maximum readings around 10000?)
264  if nearest_array_element != 'not found':
265  if DEBUG:
266  print "nearest_array_element:", nearest_array_element
267  print "nearest_array_weighted_dist: %0.3f"%nearest_array_weighted_dist
268  newreading = self.contact_depths[tip][contactnum] * \
269  self.sensor_array_info[tip].force_per_unit[nearest_array_element] * 5000.
270  if newreading > 10000:
271  newreading = 10000
272 
273  if newreading > finger_tip_readings[tip][nearest_array_element]:
274  finger_tip_readings[tip][nearest_array_element] = newreading
275 
276  self.contact_positions[tip] = []
277  self.contact_normals[tip] = []
278  self.contact_depths[tip] = []
279 
280  self.lock.release()
281 
282  if DEBUG:
283  print "left tip readings:", pplist(finger_tip_readings['l'])
284  print "right tip readings:", pplist(finger_tip_readings['r'])
285 
286  #fill in the message and publish it
287  ps = PressureState()
288  ps.header.stamp = rospy.get_rostime();
289  ps.l_finger_tip = []
290  ps.r_finger_tip = []
291  for i in range(sensor_element_count):
292  ps.l_finger_tip.append(finger_tip_readings['l'][i])
293  ps.r_finger_tip.append(finger_tip_readings['r'][i])
294  self.pub.publish(ps)
295 
296 
297 
298 
299 if __name__ == '__main__':
300 
301  rospy.init_node('sim_contact_translator', anonymous=True)
302 
305 
306  print "done initializing, starting to publish"
307 
308  while not rospy.is_shutdown():
309  rospy.sleep(0.01)
310  s1.publish()
311  s2.publish()


pr2_gazebo
Author(s): John Hsu
autogenerated on Fri May 3 2019 02:24:27