sensor_info.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 '''Quantities describing the PR2 fingertip sensors (mostly copied from sensor_info.py in fingertip_pressure/scripts)'''
00003 
00004 force_per_unit_table = [ 
00005         600, # 0 bottom
00006         400, # 1 side
00007         600, # 2 corner
00008         600, # 3 front
00009         600, # 4 front
00010         600, # 5 corner
00011         400, # 6 side
00012         1600, 1600, 1600, 
00013         1600, 1600, 1600, 
00014         1600, 1600, 1600, 
00015         1600, 1600, 1600, 
00016         1600, 1600, 1600, 
00017         ]
00018 
00019 # coordinates are in mm here, and get converted to meters for publishing.
00020 coordinates = [
00021         # center             half-side 1        half-side 2
00022         [ 29.3, 11.0,  0.0,   0.0,  0.0, 10.0,   2.8,  0.0,  0.0 ],    # 0
00023         [ 16.5,  5.2, 11.5,  12.0,  0.0,  0.0,   0.0,  3.0,  0.0 ],    # 1
00024         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 2 fused 
00025         [ 35.0,  4.7,  3.5,   0.0,  0.0,  3.5,   0.0, -2.5,  0.0 ],    # 3 CHK x
00026         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 4 mirrored 
00027         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 5 mirrored 
00028         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 6 mirrored 
00029         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 7 translated
00030         [ 30.5,  0.0,  0.0,   0.0,  0.0, -2.8,   3.0,  0.0,  0.0 ],    # 8 CHK
00031         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 9 translated 
00032         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 10
00033         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 11 translated 
00034         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 12 translated 
00035         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 13 translated 
00036         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 14 translated  
00037         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 15 translated  
00038         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 16 translated  
00039         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 17 translated  
00040         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 18 translated  
00041         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 19 translated  
00042         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 20 translated  
00043         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 21 translated  
00044         ]                                                                                 
00045 
00046 # Generate #2 to go from #1 to #3
00047 coordinates[2][0] = (coordinates[1][0] + coordinates[1][3] + coordinates[3][0]) / 2
00048 coordinates[2][1] = coordinates[3][1]
00049 coordinates[2][2] = (coordinates[1][2] + coordinates[3][2] + coordinates[3][5]) / 2
00050 coordinates[2][3] = (coordinates[1][0] + coordinates[1][3] - coordinates[3][0]) / 2
00051 coordinates[2][4] = 0
00052 coordinates[2][5] = (coordinates[1][2] - coordinates[3][2] - coordinates[3][5]) / 2
00053 for i in range(6,9):
00054     coordinates[2][i] = coordinates[3][i]
00055 
00056 # Same sensor on other side.
00057 def mirror(src, dest):
00058     coordinates[dest] = list(coordinates[src])
00059     coordinates[dest][2] = -coordinates[dest][2]
00060     coordinates[dest][5] = -coordinates[dest][5]
00061     # Flip half-edge 2 this way so that cross product points out.
00062     coordinates[dest][6] = -coordinates[dest][6]
00063     coordinates[dest][7] = -coordinates[dest][7]
00064 
00065 # Generate one sidewall from the other
00066 mirror(1, 6)
00067 mirror(2, 5)
00068 mirror(3, 4)
00069 
00070 # Step 2 * dim times half-side k.
00071 def translate(src, dest, dir, k):
00072     coordinates[dest] = list(coordinates[src])
00073     for i in range(0,3):
00074         coordinates[dest][i] = coordinates[dest][i] + 2 * dir * coordinates[dest][i + 3 * k]
00075 
00076 # Generate the main array from #8
00077 translate(8, 7, 1, 1)
00078 translate(8, 9, -1, 1)
00079 for i in range(10, 22):
00080    translate(i-3, i, -1, 2)
00081 
00082 # Adjust for actual origin and flip Z
00083 for i in range(0,22):
00084     # Move origin
00085     coordinates[i][0] = coordinates[i][0] - 4
00086     coordinates[i][1] = coordinates[i][1] - 15
00087     # Flip Z
00088     coordinates[i][2] = -coordinates[i][2]
00089     coordinates[i][5] = -coordinates[i][5]
00090     coordinates[i][6] = -coordinates[i][6]
00091     coordinates[i][7] = -coordinates[i][7]
00092 
00093 def multorientation(data, ori):
00094     for i in range(0, len(data)):
00095         data[i][1] = data[i][1] * ori
00096         data[i][2] = data[i][2] * ori
00097 
00098 def extractvec(i):
00099     out = [];
00100     for j in range(0,len(coordinates)):
00101         v = [coordinates[j][i] / 1000., \
00102         coordinates[j][i+1] / 1000., \
00103         coordinates[j][i+2] / 1000.]
00104         out.append(v)
00105     return out
00106 
00107 def pressureInformation(orientation):
00108 
00109     center = extractvec(0)
00110     halfside1 = extractvec(3)
00111     halfside2 = extractvec(6)
00112     multorientation(center, orientation)
00113     multorientation(halfside1, orientation)
00114     multorientation(halfside2, orientation)
00115     return (force_per_unit_table, center, halfside1, halfside2)
00116   
00117 
00118 #pretty-print list to string
00119 def pplist(list):
00120     return ' '.join(['%2.4f'%x for x in list])
00121 
00122 
00123 if __name__ == '__main__':
00124 
00125     left = pressureInformation(1)
00126     right = pressureInformation(-1)
00127 
00128     print "left center:"
00129     print '\n'.join([pplist(x) for x in left[1]])
00130     print "left halfside1:"
00131     print '\n'.join([pplist(x) for x in left[2]])
00132     print "left halfside2:"
00133     print '\n'.join([pplist(x) for x in left[3]])
00134 
00135     print "right center:"
00136     print '\n'.join([pplist(x) for x in right[1]])
00137     print "right halfside1:"
00138     print '\n'.join([pplist(x) for x in right[2]])
00139     print "right halfside2:"
00140     print '\n'.join([pplist(x) for x in right[3]])


pr2_gripper_reactive_approach
Author(s): Kaijen Hsiao
autogenerated on Mon Oct 6 2014 12:27:12