pr2_fingertip_pressure_contact_translator.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 
00003 #***********************************************************
00004 #* Software License Agreement (BSD License)
00005 #*
00006 #*  Copyright (c) 2009, Willow Garage, Inc.
00007 #*  All rights reserved.
00008 #*
00009 #*  Redistribution and use in source and binary forms, with or without
00010 #*  modification, are permitted provided that the following conditions
00011 #*  are met:
00012 #*
00013 #*   * Redistributions of source code must retain the above copyright
00014 #*     notice, this list of conditions and the following disclaimer.
00015 #*   * Redistributions in binary form must reproduce the above
00016 #*     copyright notice, this list of conditions and the following
00017 #*     disclaimer in the documentation and/or other materials provided
00018 #*     with the distribution.
00019 #*   * Neither the name of the Willow Garage nor the names of its
00020 #*     contributors may be used to endorse or promote products derived
00021 #*     from this software without specific prior written permission.
00022 #*
00023 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 #*  POSSIBILITY OF SUCH DAMAGE.
00035 #***********************************************************
00036 
00037 # Author: Kaijen Hsiao
00038 
00039 # Simulates the fingertip sensor array based on the bumper contact states
00040 # subscribes to Gazebo's ContactsState messages
00041 # publishes PressureState messages
00042 
00043 import roslib
00044 roslib.load_manifest('pr2_gazebo')
00045 import rospy
00046 import tf
00047 from pr2_msgs.msg import PressureState
00048 from geometry_msgs.msg import PointStamped, Vector3Stamped
00049 from gazebo_plugins.msg import ContactState, ContactsState
00050 
00051 import numpy
00052 import math
00053 import threading
00054 import pdb
00055 import time
00056 
00057 from fingertip_pressure import fingertip_geometry
00058 
00059 DEBUG = 0
00060 
00061 #vector norm of numpy 3-vector (as array)
00062 def vectnorm(vect):
00063     return math.sqrt(numpy.dot(vect, vect))
00064 
00065 #vector norm of numpy array of vectors (nx3)
00066 def arrayvectnorm(vectarray):
00067     try:
00068         return [vectnorm(x) for x in vectarray]
00069     except:
00070         pdb.set_trace()
00071 
00072 #angle diff of two numpy 3-arrays
00073 def anglediff(vect1, vect2):
00074     return math.acos(numpy.dot(vect1, vect2)/(vectnorm(vect1)*vectnorm(vect2)))
00075 
00076 #pretty-print a list of floats to str
00077 def pplist(list):
00078     return ' '.join(['%2.3f'%x for x in list])
00079 
00080 #pretty-print a list of lists of floats to str
00081 def pplistlist(list):
00082     return '\n'.join([pplist(x) for x in list])
00083 
00084 #convert a Vector3 or a Point msg to a 4x1 numpy matrix
00085 def vectorToNumpyMat(vector):
00086     return numpy.matrix([[vector.x, vector.y, vector.z, 1]]).transpose()
00087 
00088 
00089 
00090 #fingertip array element info 
00091 #(x toward tip, y away from front in l/toward front in r, z to the left in l/to the right in r)
00092 class SensorArrayInfo:
00093     def __init__(self, tip):
00094 
00095         if tip == 'l':
00096             l_msg = fingertip_geometry.pressureInformation('', 1)
00097             (self.force_per_unit, self.center, self.halfside1, self.halfside2) = \
00098                 (l_msg.force_per_unit, l_msg.center, l_msg.halfside1, l_msg.halfside2)
00099         else:
00100             r_msg = fingertip_geometry.pressureInformation('', -1)
00101             (self.force_per_unit, self.center, self.halfside1, self.halfside2) = \
00102                 (r_msg.force_per_unit, r_msg.center, r_msg.halfside1, r_msg.halfside2)
00103 
00104         #convert to numpy arrays 
00105         self.center = [numpy.array([v.x, v.y, v.z]) for v in self.center]
00106         self.halfside1 = [numpy.array([v.x, v.y, v.z]) for v in self.halfside1]
00107         self.halfside2 = [numpy.array([v.x, v.y, v.z]) for v in self.halfside2]
00108         
00109         #compute sensor element normals
00110         self.normal = numpy.cross(self.halfside1, self.halfside2)
00111         self.normal = [self.normal[i] / vectnorm(self.normal[i]) for i in range(len(self.normal))]
00112 
00113         #print "tip:", tip
00114         #print "center:", self.center
00115         #print "halfside1:", self.halfside1
00116         #print "halfside2:", self.halfside2
00117         #print "normal:", self.normal
00118 
00119 
00120 class contactArraySimulator:
00121 
00122     #gripper is 'r' or 'l'
00123     def __init__(self, gripper): 
00124         self.gripper = gripper
00125 
00126         self.sensor_array_info = {'r': SensorArrayInfo('r'), 'l': SensorArrayInfo('l')}
00127 
00128         self.lock = threading.Lock()
00129 
00130         #initialize the dictionaries for the contact positions, normals, and depths
00131         self.contact_positions = {'r':[], 'l':[]}
00132         self.contact_normals = {'r':[], 'l':[]}
00133         self.contact_depths = {'r':[], 'l':[]}
00134 
00135         #the frames for the fingertip contact array positions
00136         self.fingertip_frameid = {'r':gripper+'_gripper_r_finger_tip_link', 
00137                                   'l':gripper+'_gripper_l_finger_tip_link'}
00138 
00139         #subscribe to Gazebo fingertip contact info 
00140         rospy.Subscriber(gripper + "_gripper_l_finger_tip_bumper/state", ContactsState, self.l_contact_callback)
00141         rospy.Subscriber(gripper + "_gripper_r_finger_tip_bumper/state", ContactsState, self.r_contact_callback)
00142 
00143         #publish contact array info as PressureState messages, just like the real robot does
00144         self.pub = rospy.Publisher("pressure/"+gripper+"_gripper_motor", PressureState)
00145 
00146 
00147 
00148     #callback for receiving left fingertip ContactsState messages
00149     def l_contact_callback(self, bumperstate):
00150         self.store_contacts('l', bumperstate)
00151 
00152     #callback for receiving right fingertip ContactsState messages
00153     def r_contact_callback(self, bumperstate):
00154         self.store_contacts('r', bumperstate)
00155 
00156     #store the contact positions and normals in the fingertip frame
00157     def store_contacts(self, whichtip, bumperstate):
00158 
00159         #self.contact_positions[whichtip] = []
00160         #self.contact_normals[whichtip] = []
00161         #self.contact_depths[whichtip] = []
00162 
00163         fingertip_frameid = self.fingertip_frameid[whichtip]
00164 
00165         self.lock.acquire()        
00166         for contactstate in bumperstate.states:
00167 
00168             #transform each contact point and normal to the fingertip frame
00169             for i in range(len(contactstate.depths)):
00170 
00171                 #account for map offset of (25.7, 25.7, 0)
00172                 contact_pos = vectorToNumpyMat(contactstate.contact_positions[i])
00173                 contact_normal = vectorToNumpyMat(contactstate.contact_normals[i])
00174                 array_pos = numpy.array([contact_pos[0,0], contact_pos[1,0], contact_pos[2,0]])
00175                 array_normal = numpy.array([contact_normal[0,0], contact_normal[1,0], contact_normal[2,0]])
00176                 self.contact_positions[whichtip].append(array_pos)
00177                 self.contact_normals[whichtip].append(array_normal)
00178                 self.contact_depths[whichtip].append(contactstate.depths[i])
00179 
00180         self.lock.release()
00181 
00182         if DEBUG:
00183             print whichtip, "positions:\n", pplistlist(self.contact_positions[whichtip])
00184             print whichtip, "normals:\n", pplistlist(self.contact_normals[whichtip])
00185                                                    
00186 
00187 
00188     #publish the current simulated fingertip sensor array readings
00189     def publish(self):
00190 
00191         self.lock.acquire()
00192 
00193         #compute fingertip sensor readings from observed contacts 
00194         sensor_element_count = len(self.sensor_array_info['l'].center)
00195         finger_tip_readings = {'l':[0]*sensor_element_count, 'r':[0]*sensor_element_count}
00196         for tip in ['l','r']:
00197             for contactnum in range(len(self.contact_positions[tip])):
00198 
00199                 #figure out which sensor array element the contact falls into, if any    
00200                 nearest_array_element = 'not found'
00201                 nearest_array_weighted_dist = 1e6;
00202 
00203                 dists = []
00204                 anglediffs = []
00205                 weighteddists = []
00206 
00207                 for arrayelem in range(sensor_element_count):
00208                     
00209                     center = self.sensor_array_info[tip].center[arrayelem]
00210                     halfside1 = self.sensor_array_info[tip].halfside1[arrayelem]
00211                     halfside2 = self.sensor_array_info[tip].halfside2[arrayelem]
00212                     normal = self.sensor_array_info[tip].normal[arrayelem]
00213 
00214                     sensedpoint = self.contact_positions[tip][contactnum]
00215                     sensednormal = self.contact_normals[tip][contactnum]
00216                     
00217                     posdiff = self.contact_positions[tip][contactnum] - \
00218                         self.sensor_array_info[tip].center[arrayelem]
00219 
00220                     #distance from sensor plane (magnitude of projection onto normal)
00221                     dist_from_plane = math.fabs(numpy.dot(posdiff, sensednormal)/vectnorm(sensednormal))
00222                     
00223                     #distance from sensor element rectangle edge (- is inside)
00224                     halfside1proj = numpy.dot(posdiff, halfside1)/vectnorm(halfside1)
00225                     halfside2proj = numpy.dot(posdiff, halfside2)/vectnorm(halfside2)
00226                     halfside1dist = math.fabs(halfside1proj) - vectnorm(halfside1)
00227                     halfside2dist = math.fabs(halfside2proj) - vectnorm(halfside2)
00228                     
00229                     #find the euclidean dist from the sensor element pad
00230                     if halfside1dist < 0: 
00231                         halfside1dist = 0.
00232                     if halfside2dist < 0:
00233                         halfside2dist = 0.
00234                     dist = math.sqrt(halfside1dist**2+halfside2dist**2+dist_from_plane**2)
00235                     dists.append(dist)
00236 
00237                     #has to be at least near the element 
00238                     #(within 1 cm; interpenetration can make positions weird)
00239                     if dist > .01 and not DEBUG:
00240                         continue
00241 
00242                     #angle difference between the two normals
00243                     normal_angle_diff = anglediff(sensednormal, normal)
00244                     anglediffs.append(normal_angle_diff)
00245 
00246                     #weight the euclidean distance difference with the normal distance 
00247                     # (1.0 cm = 1 rad; don't want to be on the wrong side, but friction can
00248                     # make the angles weird) 
00249                     weighted_dist = normal_angle_diff + dist / .01
00250                     weighteddists.append(weighted_dist)
00251 
00252                     if weighted_dist < nearest_array_weighted_dist:
00253                         nearest_array_weighted_dist = weighted_dist
00254                         nearest_array_element = arrayelem
00255                 
00256                 if DEBUG:
00257                     print "tip:", tip
00258                     print "dists:", pplist(dists)
00259                     print "anglediffs:", pplist(anglediffs)
00260                     print "weighteddists:", pplist(weighteddists)
00261 
00262                 #update that sensor element's depth if this depth is greater
00263                 #(the maximum readings around 10000?)
00264                 if nearest_array_element != 'not found':
00265                     if DEBUG:
00266                         print "nearest_array_element:", nearest_array_element
00267                         print "nearest_array_weighted_dist: %0.3f"%nearest_array_weighted_dist
00268                     newreading = self.contact_depths[tip][contactnum] * \
00269                         self.sensor_array_info[tip].force_per_unit[nearest_array_element] * 5000.
00270                     if newreading > 10000:
00271                         newreading = 10000
00272 
00273                     if newreading > finger_tip_readings[tip][nearest_array_element]:
00274                         finger_tip_readings[tip][nearest_array_element] = newreading
00275 
00276             self.contact_positions[tip] = []
00277             self.contact_normals[tip] = []
00278             self.contact_depths[tip] = []
00279 
00280         self.lock.release()
00281 
00282         if DEBUG:
00283             print "left tip readings:", pplist(finger_tip_readings['l'])
00284             print "right tip readings:", pplist(finger_tip_readings['r'])
00285 
00286         #fill in the message and publish it
00287         ps = PressureState()
00288         ps.header.stamp = rospy.get_rostime();
00289         ps.l_finger_tip = []
00290         ps.r_finger_tip = []
00291         for i in range(sensor_element_count):
00292             ps.l_finger_tip.append(finger_tip_readings['l'][i])
00293             ps.r_finger_tip.append(finger_tip_readings['r'][i])
00294         self.pub.publish(ps)
00295 
00296 
00297             
00298 
00299 if __name__ == '__main__':
00300 
00301     rospy.init_node('sim_contact_translator', anonymous=True)
00302 
00303     s1 = contactArraySimulator('r')
00304     s2 = contactArraySimulator('l')
00305         
00306     print "done initializing, starting to publish"
00307 
00308     while not rospy.is_shutdown():
00309         rospy.sleep(0.01)
00310         s1.publish()
00311         s2.publish()


pr2_gazebo
Author(s): John Hsu
autogenerated on Thu Apr 24 2014 15:48:38