transform_utils.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
00004 
00005 from tf import transformations as trans
00006 from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped
00007 from copy import deepcopy
00008 import numpy as np
00009 
00010 class TransformationUtils():
00011     @classmethod
00012     def transformFromPose(cls, p):
00013         t = Transform()
00014         t.translation.x = p.position.x
00015         t.translation.y = p.position.y
00016         t.translation.z = p.position.z
00017         t.rotation = deepcopy(p.orientation)
00018         return t
00019 
00020     @classmethod
00021     def poseFromTransform(cls, t):
00022         p = Pose()
00023         p.position.x = t.translation.x
00024         p.position.y = t.translation.y
00025         p.position.z = t.translation.z
00026         p.orientation = deepcopy(t.rotation)
00027         return p
00028 
00029     @classmethod
00030     def matrixFromPosition(cls, pos):
00031         return trans.translation_matrix((pos.x, pos.y, pos.z))
00032 
00033     @classmethod
00034     def matrixFromRotation(cls, rot):
00035         return trans.quaternion_matrix((rot.x, rot.y, rot.z, rot.w))
00036 
00037     @classmethod
00038     def matrixFromPose(cls, pose):
00039         return np.dot(cls.matrixFromPosition(pose.position), cls.matrixFromRotation(pose.orientation))
00040 
00041     @classmethod
00042     def matrixFromTransform(cls, t):
00043         return np.dot(cls.matrixFromPosition(t.translation), cls.matrixFromRotation(t.rotation))
00044 
00045     @classmethod
00046     def pointFromMatrix(cls, m):
00047         return Point(*trans.translation_from_matrix(m))
00048 
00049     @classmethod
00050     def quaternionFromMatrix(cls, m):
00051         return Quaternion(*trans.quaternion_from_matrix(m))
00052 
00053     @classmethod
00054     def transformPoseWithTransformStamped(cls, p, ts):
00055         m = np.dot(cls.matrixFromTransform(ts.transform), cls.matrixFromPose(p))
00056         pos = cls.pointFromMatrix(m)
00057         quat = cls.quaternionFromMatrix(m)
00058         ps = PoseStamped()
00059         ps.header.frame_id = ts.header.frame_id
00060         ps.header.stamp = ts.header.stamp
00061         ps.pose.position = pos
00062         ps.pose.orientation = quat
00063         return ps


jsk_pr2_startup
Author(s):
autogenerated on Sat Jul 1 2017 02:43:24