kdl_posemath.py
Go to the documentation of this file.
00001 from geometry_msgs.msg import Pose, Point, Quaternion
00002 from tf import transformations
00003 import tf
00004 import rospy
00005 import numpy
00006 
00007 from PyKDL import *
00008 
00009 def fromTf(tf):
00010     """
00011     :param tf: :class:`tf.Transformer` transform
00012     :type tf: tuple (translation, quaternion)
00013     :return: New :class:`PyKDL.Frame` object
00014 
00015     Convert a pose returned by :meth:`tf.Transformer.lookupTransform` to a :class:`PyKDL.Frame`.
00016 
00017     .. doctest::
00018 
00019         >>> import rospy
00020         >>> import tf
00021         >>> import geometry_msgs.msg
00022         >>> t = tf.Transformer(True, rospy.Duration(10.0))
00023         >>> m = geometry_msgs.msg.TransformStamped()
00024         >>> m.header.frame_id = 'THISFRAME'
00025         >>> m.child_frame_id = 'CHILD'
00026         >>> m.transform.translation.x = 668.5
00027         >>> m.transform.rotation.w = 1.0
00028         >>> t.setTransform(m)
00029         >>> t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0))
00030         ((668.5, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0))
00031         >>> import tf_conversions.posemath as pm
00032         >>> p = pm.fromTf(t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0)))
00033         >>> print pm.toMsg(p * p)
00034         position: 
00035           x: 1337.0
00036           y: 0.0
00037           z: 0.0
00038         orientation: 
00039           x: 0.0
00040           y: 0.0
00041           z: 0.0
00042           w: 1.0
00043 
00044     """
00045 
00046     position, quaternion = tf
00047     x, y, z = position
00048     Qx, Qy, Qz, Qw = quaternion
00049     return Frame(Rotation.Quaternion(Qx, Qy, Qz, Qw), 
00050                  Vector(x, y, z))
00051 
00052 def toTf(f):
00053     """
00054     :param f: input pose
00055     :type f: :class:`PyKDL.Frame`
00056 
00057     Return a tuple (position, quaternion) for the pose.
00058     """
00059 
00060     return ((f.p[0], f.p[1], f.p[2]), f.M.GetQuaternion())
00061 
00062 # to and from pose message
00063 def fromMsg(p):
00064     """
00065     :param p: input pose
00066     :type p: :class:`geometry_msgs.msg.Pose`
00067     :return: New :class:`PyKDL.Frame` object
00068 
00069     Convert a pose represented as a ROS Pose message to a :class:`PyKDL.Frame`.
00070     """
00071     return Frame(Rotation.Quaternion(p.orientation.x,
00072                                      p.orientation.y,
00073                                      p.orientation.z,
00074                                      p.orientation.w),  
00075                  Vector(p.position.x, p.position.y, p.position.z))
00076 
00077 def toMsg(f):
00078     """
00079     :param f: input pose
00080     :type f: :class:`PyKDL.Frame`
00081 
00082     Return a ROS Pose message for the Frame f.
00083 
00084     """
00085     p = Pose()
00086     p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w = f.M.GetQuaternion()
00087     p.position.x = f.p[0]
00088     p.position.y = f.p[1]
00089     p.position.z = f.p[2]
00090     return p
00091 
00092 
00093 # to and from matrix
00094 def fromMatrix(m):
00095     """
00096     :param m: input 4x4 matrix
00097     :type m: :func:`numpy.array`
00098     :return: New :class:`PyKDL.Frame` object
00099 
00100     Convert a pose represented as a 4x4 numpy array to a :class:`PyKDL.Frame`.
00101 
00102     """
00103     return Frame(Rotation(m[0,0], m[0,1], m[0,2],
00104                           m[1,0], m[1,1], m[1,2],
00105                           m[2,0], m[2,1], m[2,2]),  
00106                  Vector(m[0,3], m[1, 3], m[2, 3]))
00107     
00108 def toMatrix(f):
00109     """
00110     :param f: input pose
00111     :type f: :class:`PyKDL.Frame`
00112 
00113     Return a numpy 4x4 array for the Frame F.
00114     """
00115     return numpy.array([[f.M[0,0], f.M[0,1], f.M[0,2], f.p[0]],
00116                         [f.M[1,0], f.M[1,1], f.M[1,2], f.p[1]],
00117                         [f.M[2,0], f.M[2,1], f.M[2,2], f.p[2]],
00118                         [0,0,0,1]])
00119 
00120 
00121 def toPose(x, q) :
00122     """
00123     :param x: input position
00124     :param 1: input quaternion
00125     :type p: output pose
00126 
00127     Return a ROS pose from python lists.
00128     """
00129     p = Pose()
00130     p.position.x = x[0]
00131     p.position.y = x[1]
00132     p.position.z = x[2]
00133     p.orientation.x = q[0]
00134     p.orientation.y = q[1]
00135     p.orientation.z = q[2]
00136     p.orientation.w = q[3]
00137     return p
00138 
00139 # from camera parameters
00140 def fromCameraParams(cv, rvec, tvec):
00141     """
00142     :param cv: OpenCV module
00143     :param rvec: A Rodrigues rotation vector - see :func:`Rodrigues2`
00144     :type rvec: 3x1 :class:`CvMat`
00145     :param tvec: A translation vector 
00146     :type tvec: 3x1 :class:`CvMat`
00147     :return: New :class:`PyKDL.Frame` object
00148     
00149     For use with :func:`FindExtrinsicCameraParams2`::
00150 
00151         import cv
00152         import tf_conversions.posemath as pm
00153         ...
00154         rvec = cv.CreateMat(3, 1, cv.CV_32FC1)
00155         tvec = cv.CreateMat(3, 1, cv.CV_32FC1)
00156         cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rvec, tvec)
00157         pose = pm.fromCameraParams(cv, rvec, tvec)
00158 
00159     """
00160     m = numpy.array([ [ 0, 0, 0, tvec[0,0] ],
00161                       [ 0, 0, 0, tvec[1,0] ], 
00162                       [ 0, 0, 0, tvec[2,0] ], 
00163                       [ 0, 0, 0, 1.0       ] ], dtype = numpy.float32)
00164     cv.Rodrigues2(rvec, m[:3,:3])
00165     return fromMatrix(m)
00166 
00167 
00168 class PoseMath(object):
00169     """
00170     :param msg: ROS Pose message
00171 
00172 
00173     Poses can be concatenated using ``*``:
00174 
00175     .. doctest::
00176 
00177         >>> import tf_conversions.posemath as pm
00178         >>> from math import pi
00179         >>> trans = PoseMath.fromEuler(1, 2, 3, 0, 0, 0)
00180         >>> rotate = PoseMath.fromEuler(0, 0, 0, pi / 2, 0, 0)
00181         >>> print trans * rotate
00182         position: 
00183           x: 1.0
00184           y: 2.0
00185           z: 3.0
00186         orientation: 
00187           x: 0.707106781187
00188           y: 0.0
00189           z: 0.0
00190           w: 0.707106781187
00191         >>> print rotate * trans
00192         position: 
00193           x: 1.0
00194           y: -3.0
00195           z: 2.0
00196         orientation: 
00197           x: 0.707106781187
00198           y: 0.0
00199           z: 0.0
00200           w: 0.707106781187
00201 
00202     and inverted using ``~``:
00203 
00204     .. doctest::
00205 
00206         >>> import tf_conversions.posemath as pm
00207         >>> trans = PoseMath.fromEuler(1, 2, 3, 0, 0, 0)
00208         >>> print ~trans
00209         position: 
00210           x: -1.0
00211           y: -2.0
00212           z: -3.0
00213         orientation: 
00214           x: 0.0
00215           y: 0.0
00216           z: 0.0
00217           w: 1.0
00218 
00219     """
00220 
00221     # Initializers
00222 
00223     def __init__(self, msg):
00224         self.msg = msg
00225 
00226     @staticmethod
00227     def fromMatrix(m):
00228         """
00229         :param m: 4x4 array
00230         :type m: :func:`numpy.array`
00231         :return: New PoseMath object
00232 
00233         Return a PoseMath object initialized from 4x4 matrix m
00234 
00235         .. doctest::
00236 
00237             >>> import numpy
00238             >>> import tf_conversions.posemath as pm
00239             >>> print PoseMath.fromMatrix(numpy.identity(4))
00240             position: 
00241               x: 0.0
00242               y: 0.0
00243               z: 0.0
00244             orientation: 
00245               x: 0.0
00246               y: 0.0
00247               z: 0.0
00248               w: 1.0
00249 
00250         """
00251         (x, y, z) = (m[0, 3], m[1, 3], m[2, 3])
00252         q = transformations.quaternion_from_matrix(m)
00253         return PoseMath(Pose(Point(x, y, z), Quaternion(*q)))
00254 
00255 
00256     @staticmethod
00257     def fromTf(tf):
00258         """
00259         :param tf: tf transform
00260         :type tf: tuple (translation, quaternion)
00261         :return: New PoseMath object
00262 
00263         Return a PoseMath object initialized from a :mod:`tf` transform, as returned by :meth:`tf.Transformer.lookupTransform`.
00264 
00265         .. doctest::
00266 
00267             >>> import rospy
00268             >>> import tf
00269             >>> import geometry_msgs.msg
00270             >>> t = tf.Transformer(True, rospy.Duration(10.0))
00271             >>> m = geometry_msgs.msg.TransformStamped()
00272             >>> m.header.frame_id = 'THISFRAME'
00273             >>> m.child_frame_id = 'CHILD'
00274             >>> m.transform.translation.x = 668.5
00275             >>> m.transform.rotation.w = 1.0
00276             >>> t.setTransform(m)
00277             >>> t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0))
00278             ((668.5, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0))
00279             >>> import tf_conversions.posemath as pm
00280             >>> p = PoseMath.fromTf(t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0)))
00281             >>> print p * p
00282             position: 
00283               x: 1337.0
00284               y: 0.0
00285               z: 0.0
00286             orientation: 
00287               x: 0.0
00288               y: 0.0
00289               z: 0.0
00290               w: 1.0
00291 
00292         """
00293         position, quaternion = tf
00294         return PoseMath(Pose(Point(*position), Quaternion(*quaternion)))
00295 
00296     @staticmethod
00297     def fromEuler(x, y, z, Rx, Ry, Rz):
00298         """
00299         :param x: x translation
00300         :type x: float 
00301         :param y: y translation
00302         :type y: float 
00303         :param z: z translation
00304         :type z: float 
00305         :param Rx: rotation around x-axis in radians
00306         :type Rx: float 
00307         :param Ry: rotation around y-axis in radians
00308         :type Ry: float 
00309         :param Rz: rotation around z-axis in radians
00310         :type Rz: float 
00311         :return: New PoseMath object
00312 
00313         Return a PoseMath object initialized from translation (x, y, z) and Euler rotation (Rx, Ry, Rz).
00314 
00315         .. doctest::
00316 
00317             >>> import tf_conversions.posemath as pm
00318             >>> from math import pi
00319             >>> print PoseMath.fromEuler(1, 2, 3, 0, pi / 2, 0)
00320             position: 
00321               x: 1
00322               y: 2
00323               z: 3
00324             orientation: 
00325               x: 0.0
00326               y: 0.707106781187
00327               z: 0.0
00328               w: 0.707106781187
00329 
00330 
00331         """
00332         q = transformations.quaternion_from_euler(Rx, Ry, Rz)
00333         return PoseMath(Pose(Point(x, y, z), Quaternion(*q)))
00334 
00335     @staticmethod
00336     def fromCameraParams(self, cv, rvec, tvec):
00337         """
00338         :param cv: OpenCV module
00339         :param rvec: A Rodrigues rotation vector - see :func:`Rodrigues2`
00340         :type rvec: 3x1 :class:`CvMat`
00341         :param tvec: A translation vector 
00342         :type tvec: 3x1 :class:`CvMat`
00343         :return: New PoseMath object
00344         
00345         For use with :func:`FindExtrinsicCameraParams2`::
00346 
00347             import cv
00348             import tf_conversions.posemath as pm
00349             ...
00350             rvec = cv.CreateMat(3, 1, cv.CV_32FC1)
00351             tvec = cv.CreateMat(3, 1, cv.CV_32FC1)
00352             cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rvec, tvec)
00353             pose = PoseMath.fromCameraParams(cv, rvec, tvec)
00354 
00355         """
00356         m = numpy.array([ [ 0, 0, 0, tvec[0,0] ],
00357                           [ 0, 0, 0, tvec[1,0] ], 
00358                           [ 0, 0, 0, tvec[2,0] ], 
00359                           [ 0, 0, 0, 1.0       ] ], dtype = numpy.float32)
00360         cv.Rodrigues2(rvec, m[:3,:3])
00361         return self.fromMatrix(m)
00362 
00363     # Operators
00364 
00365     def __mul__(self, other):
00366         m = numpy.dot(self.asMatrix(), other.asMatrix())
00367         return PoseMath.fromMatrix(m)
00368 
00369     def __invert__(self):
00370         inv = numpy.linalg.inv(self.asMatrix())
00371         return PoseMath.fromMatrix(inv)
00372 
00373     # Representations
00374 
00375     def __repr__(self):
00376         return repr(self.msg)
00377 
00378     def asMessage(self):
00379         """ Return the pose as a ROS ``Pose`` message """
00380         return self.msg
00381 
00382     def asMatrix(self):
00383         """ Return a numpy 4x4 array for the pose. """
00384         translation = (self.msg.position.x, self.msg.position.y, self.msg.position.z)
00385         rotation = (self.msg.orientation.x, self.msg.orientation.y, self.msg.orientation.z, self.msg.orientation.w)
00386         return numpy.dot(transformations.translation_matrix(translation), transformations.quaternion_matrix(rotation))        
00387 
00388     def asEuler(self):
00389         """ Return a tuple (x, y, z, Rx, Ry, Rz) for the pose. """
00390         tmp = transformations.euler_from_quaternion((self.msg.orientation.x, self.msg.orientation.y, self.msg.orientation.z, self.msg.orientation.w))
00391         return (self.msg.position.x, self.msg.position.y, self.msg.position.z, tmp[0], tmp[1], tmp[2])
00392 
00393     def asTf(self):
00394         """ Return a tuple (position, quaternion) for the pose. """
00395         p = self.msg.position
00396         q = self.msg.orientation
00397         return ((p.x, p.y, p.z), (q.x, q.y, q.z, q.w))


r2_teleop
Author(s): Paul Dinh
autogenerated on Mon Oct 6 2014 02:43:06