fingertip_geometry.py
Go to the documentation of this file.
00001 #***********************************************************
00002 #* Software License Agreement (BSD License)
00003 #*
00004 #*  Copyright (c) 2009, Willow Garage, Inc.
00005 #*  All rights reserved.
00006 #*
00007 #*  Redistribution and use in source and binary forms, with or without
00008 #*  modification, are permitted provided that the following conditions
00009 #*  are met:
00010 #*
00011 #*   * Redistributions of source code must retain the above copyright
00012 #*     notice, this list of conditions and the following disclaimer.
00013 #*   * Redistributions in binary form must reproduce the above
00014 #*     copyright notice, this list of conditions and the following
00015 #*     disclaimer in the documentation and/or other materials provided
00016 #*     with the distribution.
00017 #*   * Neither the name of the Willow Garage nor the names of its
00018 #*     contributors may be used to endorse or promote products derived
00019 #*     from this software without specific prior written permission.
00020 #*
00021 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 #*  POSSIBILITY OF SUCH DAMAGE.
00033 #***********************************************************
00034 
00035 # Author: Blaise Gassend
00036 
00037 import roslib
00038 roslib.load_manifest('fingertip_pressure')
00039 
00040 from fingertip_pressure.msg import PressureInfoElement
00041 from geometry_msgs.msg import Vector3
00042 
00043 force_per_unit_table = [ 
00044         600, # 0 bottom
00045         400, # 1 side
00046         600, # 2 corner
00047         600, # 3 front
00048         600, # 4 front
00049         600, # 5 corner
00050         400, # 6 side
00051         1600, 1600, 1600, 
00052         1600, 1600, 1600, 
00053         1600, 1600, 1600, 
00054         1600, 1600, 1600, 
00055         1600, 1600, 1600, 
00056         ]
00057 
00058 # coordinates are in mm here, and get converted to meters for publishing.
00059 coordinates = [
00060         # center             half-side 1        half-side 2
00061         [ 29.3, 11.0,  0.0,   0.0,  0.0, 10.0,   2.8,  0.0,  0.0 ],    # 0
00062         [ 16.5,  5.2, 11.5,  12.0,  0.0,  0.0,   0.0,  3.0,  0.0 ],    # 1
00063         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 2 fused 
00064         [ 35.0,  4.7,  3.5,   0.0,  0.0,  3.5,   0.0, -2.5,  0.0 ],    # 3 CHK x
00065         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 4 mirrored 
00066         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 5 mirrored 
00067         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 6 mirrored 
00068         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 7 translated
00069         [ 30.5,  0.0,  0.0,   0.0,  0.0, -2.8,   3.0,  0.0,  0.0 ],    # 8 CHK
00070         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 9 translated 
00071         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 10
00072         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 11 translated 
00073         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 12 translated 
00074         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 13 translated 
00075         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 14 translated  
00076         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 15 translated  
00077         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 16 translated  
00078         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 17 translated  
00079         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 18 translated  
00080         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 19 translated  
00081         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 20 translated  
00082         [  0.0,  0.0,  0.0,   0.0,  0.0,  0.0,   0.0,  0.0,  0.0 ],    # 21 translated  
00083         ]                                                                                 
00084 
00085 # Generate #2 to go from #1 to #3
00086 coordinates[2][0] = (coordinates[1][0] + coordinates[1][3] + coordinates[3][0]) / 2
00087 coordinates[2][1] = coordinates[3][1]
00088 coordinates[2][2] = (coordinates[1][2] + coordinates[3][2] + coordinates[3][5]) / 2
00089 coordinates[2][3] = (coordinates[1][0] + coordinates[1][3] - coordinates[3][0]) / 2
00090 coordinates[2][4] = 0
00091 coordinates[2][5] = (coordinates[1][2] - coordinates[3][2] - coordinates[3][5]) / 2
00092 for i in range(6,9):
00093     coordinates[2][i] = coordinates[3][i]
00094 
00095 # Same sensor on other side.
00096 def mirror(src, dest):
00097     coordinates[dest] = list(coordinates[src])
00098     coordinates[dest][2] = -coordinates[dest][2]
00099     coordinates[dest][5] = -coordinates[dest][5]
00100     # Flip half-edge 2 this way so that cross product points out.
00101     coordinates[dest][6] = -coordinates[dest][6]
00102     coordinates[dest][7] = -coordinates[dest][7]
00103 
00104 # Generate one sidewall from the other
00105 mirror(1, 6)
00106 mirror(2, 5)
00107 mirror(3, 4)
00108 
00109 # Step 2 * dim times half-side k.
00110 def translate(src, dest, dir, k):
00111     coordinates[dest] = list(coordinates[src])
00112     for i in range(0,3):
00113         coordinates[dest][i] = coordinates[dest][i] + 2 * dir * coordinates[dest][i + 3 * k]
00114 
00115 # Generate the main array from #8
00116 translate(8, 7, 1, 1)
00117 translate(8, 9, -1, 1)
00118 for i in range(10, 22):
00119    translate(i-3, i, -1, 2)
00120 
00121 # Adjust for actual origin and flip Z
00122 for i in range(0,22):
00123     # Move origin
00124     coordinates[i][0] = coordinates[i][0] - 4
00125     coordinates[i][1] = coordinates[i][1] - 15
00126     # Flip Z
00127     coordinates[i][2] = -coordinates[i][2]
00128     coordinates[i][5] = -coordinates[i][5]
00129     coordinates[i][6] = -coordinates[i][6]
00130     coordinates[i][7] = -coordinates[i][7]
00131 
00132 def multorientation(data, ori):
00133     for i in range(0, len(data)):
00134         data[i].y = data[i].y * ori
00135         data[i].z = data[i].z * ori
00136 
00137 def extractvec(i):
00138     out = [];
00139     for j in range(0,len(coordinates)):
00140         v = Vector3()
00141         v.x = coordinates[j][i] / 1000.;
00142         v.y = coordinates[j][i+1] / 1000.;
00143         v.z = coordinates[j][i+2] / 1000.;
00144         out.append(v)
00145     return out
00146 
00147 def pressureInformation(frame_id, orientation):
00148     msg = PressureInfoElement()
00149     msg.frame_id = frame_id
00150     msg.force_per_unit = force_per_unit_table
00151     msg.center = extractvec(0)
00152     msg.halfside1 = extractvec(3)
00153     msg.halfside2 = extractvec(6)
00154     multorientation(msg.center, orientation)
00155     multorientation(msg.halfside1, orientation)
00156     multorientation(msg.halfside2, orientation)
00157     #print "%e %e %e  %e %e %e  %e %e %e"%(msg.center[7].x, msg.center[7].y,
00158     #        msg.center[7].z, msg.halfside1[7].x, msg.halfside1[7].y,
00159     #        msg.halfside1[7].z, msg.halfside2[7].x, msg.halfside2[7].y,
00160     #        msg.halfside2[7].z)
00161     return msg


fingertip_pressure
Author(s): Blaise Gassend/blaise@willowgarage.com
autogenerated on Thu Jan 2 2014 11:39:26