single_transform.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 
00033 import numpy
00034 from numpy import matrix, vsplit, sin, cos, reshape, zeros, pi
00035 import rospy
00036 
00037 import PyKDL
00038 import yaml, math
00039 
00040 # This represents the transform for a single joint in the URDF
00041 # parameters are x,y,z and rotation as angle-axis
00042 
00043 param_names = ['x','y','z','a','b','c']
00044 
00045 class SingleTransform:
00046     def __init__(self, config = [0, 0, 0, 0, 0, 0], name=""):
00047         self._name = name
00048         eval_config = [eval(str(x)) for x in config]
00049         self._config = reshape(matrix(eval_config, float), (-1,1))
00050 
00051         rospy.logdebug("Initializing single transform %s with params [%s]", name, ", ".join(["% 2.4f" % x for x in eval_config]))
00052         self.inflate(self._config)
00053 
00054     def calc_free(self, free_config):
00055         return [x == 1 for x in free_config]
00056 
00057     def params_to_config(self, param_vec):
00058         return param_vec.T.tolist()[0]
00059 
00060     # Convert column vector of params into config
00061     def inflate(self, p):
00062         self._config = p.copy()  # Once we can back compute p from T, we don't need _config
00063 
00064         # Init output matrix
00065         T = matrix( zeros((4,4,), float))
00066         T[3,3] = 1.0
00067         
00068         # Copy position into matrix
00069         T[0:3,3] = p[0:3,0]
00070         
00071         # Renormalize the rotation axis to be unit length
00072         U,S,Vt = numpy.linalg.svd(p[3:6,0])
00073         a = U[:,0]
00074         rot_angle = S[0]*Vt[0,0]
00075         
00076         # Build rotation matrix
00077         c = cos(rot_angle)
00078         s = sin(rot_angle)
00079         R = matrix( [ [   a[0,0]**2+(1-a[0,0]**2)*c, a[0,0]*a[1,0]*(1-c)-a[2,0]*s, a[0,0]*a[2,0]*(1-c)+a[1,0]*s],
00080                       [a[0,0]*a[1,0]*(1-c)+a[2,0]*s,    a[1,0]**2+(1-a[1,0]**2)*c, a[1,0]*a[2,0]*(1-c)-a[0,0]*s],
00081                       [a[0,0]*a[2,0]*(1-c)-a[1,0]*s, a[1,0]*a[2,0]*(1-c)+a[0,0]*s,    a[2,0]**2+(1-a[2,0]**2)*c] ] )
00082 
00083         T[0:3,0:3] = R
00084         self.transform = T
00085 
00086     # Take transform, and convert into 6 param vector
00087     def deflate(self):
00088         # todo: This currently a hacky stub. To be correct, this should infer the parameter vector from the 4x4 transform
00089         return self._config
00090 
00091     # Returns # of params needed for inflation & deflation
00092     def get_length(self):
00093         return len(param_names)
00094 
00095 
00096 # Convert from rotation-axis-with-angle-as-magnitude representation to Euler RPY
00097 def angle_axis_to_RPY(vec):
00098     angle = math.sqrt(sum([vec[i]**2 for i in range(3)]))
00099     hsa = math.sin(angle/2.)
00100     if epsEq(angle, 0):
00101         return (0.,0.,0.)
00102     quat = [vec[0]/angle*hsa, vec[1]/angle*hsa, vec[2]/angle*hsa, math.cos(angle/2.)]
00103     rpy = quat_to_rpy(quat)
00104     return rpy
00105 
00106 # Convert from Euler RPY to rotation-axis-with-angle-as-magnitude
00107 def RPY_to_angle_axis(vec):
00108     if epsEq(vec[0], 0) and epsEq(vec[1], 0) and epsEq(vec[2], 0):
00109         return [0.0, 0.0, 0.0]
00110     quat = rpy_to_quat(vec)
00111     angle = math.acos(quat[3])*2.0
00112     hsa = math.sin(angle/2.)
00113     axis = [quat[0]/hsa*angle, quat[1]/hsa*angle, quat[2]/hsa*angle]
00114     return axis
00115     
00116 def rpy_to_quat(rpy):
00117     return PyKDL.Rotation.RPY(rpy[0], rpy[1], rpy[2]).GetQuaternion()
00118 
00119 def quat_to_rpy(q):
00120     return PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3]).GetRPY()
00121 
00122 #return 1 if value1 and value2 are within eps of each other, 0 otherwise
00123 def epsEq(value1, value2, eps = 1e-10):
00124     if math.fabs(value1-value2) <= eps:
00125         return 1
00126     return 0
00127 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Tue Sep 27 2016 04:06:33