dummyScanner_coord_frames.py
Go to the documentation of this file.
00001 #
00002 # Copyright (c) 2010, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 
00028 ''' Coordinate frames for testing ROS-interface (copied from Cody).
00029 '''
00030 
00031 import numpy as np, math
00032 import copy
00033 import hrl_lib.transforms as tr
00034 
00035 # dictionary for transforming different coorsinate frames to global coord frame
00036 # global is NOT the world or fixed frame, its just a convenient global frame
00037 _globalT = {
00038     'torso' : None,
00039     'thok0' : None,
00040     'utm0' : None,
00041     'utmcam0': None,
00042     'mecanum': None
00043 }
00044 
00045 def create_globalTDict():
00046     """ call the create functions for all the coord frames
00047     """
00048     createTorsoTransform()
00049     createThok0Transform()
00050     createUtm0Transform()
00051     createMecanumTransform()
00052 
00053 def createTorsoTransform():
00054     ''' torso frame -> global frame
00055     '''
00056     disp = np.matrix([0.,0.,0.]).T
00057     rot = np.matrix(np.eye(3))
00058     t = tr.composeHomogeneousTransform(rot,disp)
00059     _globalT['torso'] = t
00060 
00061 def createThok0Transform():
00062     ''' thok0 frame -> global frame
00063     '''
00064     disp = np.matrix([0.,0.,0.09]).T
00065     rot = np.matrix(np.eye(3))
00066     t = tr.composeHomogeneousTransform(rot,disp)
00067     _globalT['thok0'] = t
00068 
00069 def createUtm0Transform():
00070     ''' utm0 frame -> global frame
00071     '''
00072     disp = copy.copy(tr.getDispSubMat(_globalT['thok0']))
00073     disp[2,0] += 0.055
00074     rot = np.matrix(np.eye(3))
00075     t = tr.composeHomogeneousTransform(rot,disp)
00076     _globalT['utm0'] = t
00077 
00078 def createMecanumTransform():
00079     ''' mecanum frame -> global frame (ignores the zenither)
00080     '''
00081     disp = np.matrix([-0.25,0.,0.0]).T
00082     rot = np.matrix(np.eye(3))
00083     t = tr.composeHomogeneousTransform(rot,disp)
00084     _globalT['mecanum'] = t
00085 
00086 create_globalTDict()
00087 
00088 
00089 def globalTmecanum(p,floating_vector=False):
00090     ''' 3x1 vector from mecanum to global.
00091     '''
00092     p_hom = tr.xyzToHomogenous(p, floating_vector)
00093     p_gl = _globalT['mecanum'] * p_hom
00094     if floating_vector == False:
00095         return p_gl[0:3]/p_gl[3]
00096     else:
00097         return p_gl[0:3]
00098 
00099 def mecanumTglobal(p,floating_vector=False):
00100     ''' 3x1 vector from global to mecanum.
00101     '''
00102     p_hom = tr.xyzToHomogenous(p, floating_vector)
00103     p_gl = tr.invertHomogeneousTransform(_globalT['mecanum']) * p_hom
00104     if floating_vector == False:
00105         return p_gl[0:3]/p_gl[3]
00106     else:
00107         return p_gl[0:3]
00108 
00109 def globalTtorso(p,floating_vector=False):
00110     ''' 3x1 vector from torso to global.
00111     '''
00112     p_hom = tr.xyzToHomogenous(p, floating_vector)
00113     p_gl = _globalT['torso'] * p_hom
00114     if floating_vector == False:
00115         return p_gl[0:3]/p_gl[3]
00116     else:
00117         return p_gl[0:3]
00118 
00119 def torsoTglobal(p,floating_vector=False):
00120     ''' 3x1 vector from global to torso.
00121     '''
00122     p_hom = tr.xyzToHomogenous(p, floating_vector)
00123     p_gl = tr.invertHomogeneousTransform(_globalT['torso']) * p_hom
00124     if floating_vector == False:
00125         return p_gl[0:3]/p_gl[3]
00126     else:
00127         return p_gl[0:3]
00128 
00129 def globalTthok0(p,floating_vector=False):
00130     ''' 3x1 vector from thok0 to global.
00131     '''
00132     p_hom = tr.xyzToHomogenous(p, floating_vector)
00133     p_gl = _globalT['thok0'] * p_hom
00134     if floating_vector == False:
00135         return p_gl[0:3]/p_gl[3]
00136     else:
00137         return p_gl[0:3]
00138 
00139 def thok0Tglobal(p,floating_vector=False):
00140     ''' 3x1 vector from global to thok0.
00141     '''
00142     p_hom = tr.xyzToHomogenous(p, floating_vector)
00143     p_gl = tr.invertHomogeneousTransform(_globalT['thok0']) * p_hom
00144     if floating_vector == False:
00145         return p_gl[0:3]/p_gl[3]
00146     else:
00147         return p_gl[0:3]
00148 
00149 def globalTutm0(p,floating_vector=False):
00150     ''' 3x1 vector from utm0 to global.
00151     '''
00152     p_hom = tr.xyzToHomogenous(p, floating_vector)
00153     p_gl = _globalT['utm0'] * p_hom
00154     if floating_vector == False:
00155         return p_gl[0:3]/p_gl[3]
00156     else:
00157         return p_gl[0:3]
00158 
00159 def utm0Tglobal(p,floating_vector=False):
00160     ''' 3x1 vector from global to utm0.
00161     '''
00162     p_hom = tr.xyzToHomogenous(p, floating_vector)
00163     p_gl = tr.invertHomogeneousTransform(_globalT['utm0']) * p_hom
00164     if floating_vector == False:
00165         return p_gl[0:3]/p_gl[3]
00166     else:
00167         return p_gl[0:3]
00168 
00169 ## transformation matrix to go from global to utmcam0 coord frame.
00170 # @param ang - servo angle (in RADIANS)
00171 # @return 4x4 transformation matrix.
00172 def utmcam0Tglobal_mat(ang):
00173     thok0Tglobal_mat = tr.invertHomogeneousTransform(_globalT['thok0'])
00174     # servo angle.
00175     disp = np.matrix([0.,0.,0.]).T
00176     tmat = tr.composeHomogeneousTransform(tr.Ry(ang),disp)*thok0Tglobal_mat
00177 
00178     # cameraTlaser from thok_cam_calib.py
00179     x = 0.012
00180     y = -0.056
00181     z = 0.035
00182     r1 = 0.
00183     r2 = 0.
00184     r3 = -0.7
00185     disp = np.matrix([-x,-y,-z]).T
00186     r = tr.Rz(math.radians(-90))*tr.Ry(math.radians(90.))
00187     disp = r*disp
00188     r = r*tr.Rx(math.radians(r1))
00189     r = r*tr.Ry(math.radians(r2))
00190     r = r*tr.Rz(math.radians(r3))
00191 
00192     t = tr.composeHomogeneousTransform(r, disp)
00193     tmat = t*tmat
00194     return tmat
00195 
00196 ## global to utmcam0 coord frame.
00197 # @param p - 3xN np matrix.
00198 # @param ang - servo angle (in RADIANS)
00199 # @param floating_vector - interpretation of p. False -> position vector. True -> floating vector (rotation only).
00200 # @return 3xN np matrix in the new coord frame.
00201 def utmcam0Tglobal(p,ang,floating_vector=False):
00202     t = utmcam0Tglobal_mat(ang)
00203     p_hom = tr.xyzToHomogenous(p, floating_vector)
00204     p_c = t * p_hom
00205     if floating_vector == False:
00206         pt = p_c[0:3]/p_c[3]
00207     else:
00208         pt = p_c[0:3]
00209 
00210     return pt
00211 
00212 ## utmcam0 coord frame to global
00213 # @param p - 3xN np matrix.
00214 # @param ang - servo angle (in RADIANS)
00215 # @param floating_vector - interpretation of p. False -> position vector. True -> floating vector (rotation only).
00216 # @return 3xN np matrix in the new coord frame.
00217 def globalTutmcam0(p,ang,floating_vector=False):
00218     t = utmcam0Tglobal_mat(ang)
00219     t = tr.invertHomogeneousTransform(t)
00220     p_hom = tr.xyzToHomogenous(p, floating_vector)
00221     p_c = t * p_hom
00222     if floating_vector == False:
00223         pt = p_c[0:3]/p_c[3]
00224     else:
00225         pt = p_c[0:3]
00226 
00227     return pt
00228 


laser_camera_segmentation
Author(s): Martin Schuster, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:56:44