fingertip_geometry.py
Go to the documentation of this file.
1 #***********************************************************
2 #* Software License Agreement (BSD License)
3 #*
4 #* Copyright (c) 2009, Willow Garage, Inc.
5 #* All rights reserved.
6 #*
7 #* Redistribution and use in source and binary forms, with or without
8 #* modification, are permitted provided that the following conditions
9 #* are met:
10 #*
11 #* * Redistributions of source code must retain the above copyright
12 #* notice, this list of conditions and the following disclaimer.
13 #* * Redistributions in binary form must reproduce the above
14 #* copyright notice, this list of conditions and the following
15 #* disclaimer in the documentation and/or other materials provided
16 #* with the distribution.
17 #* * Neither the name of the Willow Garage nor the names of its
18 #* contributors may be used to endorse or promote products derived
19 #* from this software without specific prior written permission.
20 #*
21 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 #* POSSIBILITY OF SUCH DAMAGE.
33 #***********************************************************
34 
35 # Author: Blaise Gassend
36 
37 import roslib
38 roslib.load_manifest('fingertip_pressure')
39 
40 from fingertip_pressure.msg import PressureInfoElement
41 from geometry_msgs.msg import Vector3
42 
43 force_per_unit_table = [
44  600, # 0 bottom
45  400, # 1 side
46  600, # 2 corner
47  600, # 3 front
48  600, # 4 front
49  600, # 5 corner
50  400, # 6 side
51  1600, 1600, 1600,
52  1600, 1600, 1600,
53  1600, 1600, 1600,
54  1600, 1600, 1600,
55  1600, 1600, 1600,
56  ]
57 
58 # coordinates are in mm here, and get converted to meters for publishing.
59 coordinates = [
60  # center half-side 1 half-side 2
61  [ 29.3, 11.0, 0.0, 0.0, 0.0, 10.0, 2.8, 0.0, 0.0 ], # 0
62  [ 16.5, 5.2, 11.5, 12.0, 0.0, 0.0, 0.0, 3.0, 0.0 ], # 1
63  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 2 fused
64  [ 35.0, 4.7, 3.5, 0.0, 0.0, 3.5, 0.0, -2.5, 0.0 ], # 3 CHK x
65  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 4 mirrored
66  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 5 mirrored
67  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 6 mirrored
68  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 7 translated
69  [ 30.5, 0.0, 0.0, 0.0, 0.0, -2.8, 3.0, 0.0, 0.0 ], # 8 CHK
70  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 9 translated
71  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 10
72  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 11 translated
73  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 12 translated
74  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 13 translated
75  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 14 translated
76  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 15 translated
77  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 16 translated
78  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 17 translated
79  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 18 translated
80  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 19 translated
81  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 20 translated
82  [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], # 21 translated
83  ]
84 
85 # Generate #2 to go from #1 to #3
86 coordinates[2][0] = (coordinates[1][0] + coordinates[1][3] + coordinates[3][0]) / 2
87 coordinates[2][1] = coordinates[3][1]
88 coordinates[2][2] = (coordinates[1][2] + coordinates[3][2] + coordinates[3][5]) / 2
89 coordinates[2][3] = (coordinates[1][0] + coordinates[1][3] - coordinates[3][0]) / 2
90 coordinates[2][4] = 0
91 coordinates[2][5] = (coordinates[1][2] - coordinates[3][2] - coordinates[3][5]) / 2
92 for i in range(6,9):
93  coordinates[2][i] = coordinates[3][i]
94 
95 # Same sensor on other side.
96 def mirror(src, dest):
97  coordinates[dest] = list(coordinates[src])
98  coordinates[dest][2] = -coordinates[dest][2]
99  coordinates[dest][5] = -coordinates[dest][5]
100  # Flip half-edge 2 this way so that cross product points out.
101  coordinates[dest][6] = -coordinates[dest][6]
102  coordinates[dest][7] = -coordinates[dest][7]
103 
104 # Generate one sidewall from the other
105 mirror(1, 6)
106 mirror(2, 5)
107 mirror(3, 4)
108 
109 # Step 2 * dim times half-side k.
110 def translate(src, dest, dir, k):
111  coordinates[dest] = list(coordinates[src])
112  for i in range(0,3):
113  coordinates[dest][i] = coordinates[dest][i] + 2 * dir * coordinates[dest][i + 3 * k]
114 
115 # Generate the main array from #8
116 translate(8, 7, 1, 1)
117 translate(8, 9, -1, 1)
118 for i in range(10, 22):
119  translate(i-3, i, -1, 2)
120 
121 # Adjust for actual origin and flip Z
122 for i in range(0,22):
123  # Move origin
124  coordinates[i][0] = coordinates[i][0] - 4
125  coordinates[i][1] = coordinates[i][1] - 15
126  # Flip Z
127  coordinates[i][2] = -coordinates[i][2]
128  coordinates[i][5] = -coordinates[i][5]
129  coordinates[i][6] = -coordinates[i][6]
130  coordinates[i][7] = -coordinates[i][7]
131 
132 def multorientation(data, ori):
133  for i in range(0, len(data)):
134  data[i].y = data[i].y * ori
135  data[i].z = data[i].z * ori
136 
137 def extractvec(i):
138  out = [];
139  for j in range(0,len(coordinates)):
140  v = Vector3()
141  v.x = coordinates[j][i] / 1000.;
142  v.y = coordinates[j][i+1] / 1000.;
143  v.z = coordinates[j][i+2] / 1000.;
144  out.append(v)
145  return out
146 
147 def pressureInformation(frame_id, orientation):
148  msg = PressureInfoElement()
149  msg.frame_id = frame_id
150  msg.force_per_unit = force_per_unit_table
151  msg.center = extractvec(0)
152  msg.halfside1 = extractvec(3)
153  msg.halfside2 = extractvec(6)
154  multorientation(msg.center, orientation)
155  multorientation(msg.halfside1, orientation)
156  multorientation(msg.halfside2, orientation)
157  #print "%e %e %e %e %e %e %e %e %e"%(msg.center[7].x, msg.center[7].y,
158  # msg.center[7].z, msg.halfside1[7].x, msg.halfside1[7].y,
159  # msg.halfside1[7].z, msg.halfside2[7].x, msg.halfside2[7].y,
160  # msg.halfside2[7].z)
161  return msg
fingertip_pressure.fingertip_geometry.extractvec
def extractvec(i)
Definition: fingertip_geometry.py:137
fingertip_pressure.fingertip_geometry.mirror
def mirror(src, dest)
Definition: fingertip_geometry.py:96
fingertip_pressure.fingertip_geometry.multorientation
def multorientation(data, ori)
Definition: fingertip_geometry.py:132
fingertip_pressure.fingertip_geometry.pressureInformation
def pressureInformation(frame_id, orientation)
Definition: fingertip_geometry.py:147
fingertip_pressure.fingertip_geometry.translate
def translate(src, dest, dir, k)
Definition: fingertip_geometry.py:110

fingertip_pressure
Author(s): Blaise Gassend
autogenerated on Thu Mar 4 2021 03:10:25