pose_converter.py
Go to the documentation of this file.
00001 import numpy as np
00002 import copy
00003 
00004 import roslib; roslib.load_manifest('hrl_generic_arms')
00005 import rospy
00006 from std_msgs.msg import Header
00007 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, PointStamped
00008 from geometry_msgs.msg import Transform, TransformStamped, Vector3
00009 from geometry_msgs.msg import Twist, TwistStamped
00010 import tf.transformations as tf_trans
00011 
00012 class PoseConverter:
00013     @staticmethod
00014     def _make_generic(args):
00015         if type(args[0]) == str:
00016             frame_id = args[0]
00017             header, homo_mat, rot_quat, rot_euler = PoseConverter._make_generic(args[1:])
00018             if header is None:
00019                 header = [0, rospy.Time.now(), '']
00020             header[2] = frame_id
00021             return header, homo_mat, rot_quat, rot_euler
00022 
00023         if len(args) == 1:
00024             if type(args[0]) is Pose:
00025                 homo_mat, rot_quat, rot_euler = PoseConverter._extract_pose_msg(args[0])
00026                 return None, homo_mat, rot_quat, rot_euler
00027 
00028             elif type(args[0]) is PoseStamped:
00029                 homo_mat, rot_quat, rot_euler = PoseConverter._extract_pose_msg(args[0].pose)
00030                 seq = args[0].header.seq
00031                 stamp = args[0].header.stamp
00032                 frame_id = args[0].header.frame_id
00033                 return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
00034 
00035             elif type(args[0]) is Transform:
00036                 homo_mat, rot_quat, rot_euler = PoseConverter._extract_tf_msg(args[0])
00037                 return None, homo_mat, rot_quat, rot_euler
00038 
00039             elif type(args[0]) is TransformStamped:
00040                 homo_mat, rot_quat, rot_euler = PoseConverter._extract_tf_msg(args[0].transform)
00041                 seq = args[0].header.seq
00042                 stamp = args[0].header.stamp
00043                 frame_id = args[0].header.frame_id
00044                 return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
00045                 
00046             elif type(args[0]) is Twist:
00047                 homo_mat, rot_quat, rot_euler = PoseConverter._extract_twist_msg(args[0])
00048                 return None, homo_mat, rot_quat, rot_euler
00049 
00050             elif type(args[0]) is TwistStamped:
00051                 homo_mat, rot_quat, rot_euler = PoseConverter._extract_twist_msg(args[0].twist)
00052                 seq = args[0].header.seq
00053                 stamp = args[0].header.stamp
00054                 frame_id = args[0].header.frame_id
00055                 return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
00056 
00057             elif isinstance(args[0], (np.matrix, np.ndarray)) and np.shape(args[0]) == (4, 4):
00058                 return (None, np.mat(args[0]), tf_trans.quaternion_from_matrix(args[0]).tolist(),
00059                         tf_trans.euler_from_matrix(args[0]))
00060 
00061             elif isinstance(args[0], (tuple, list)) and len(args[0]) == 2:
00062                 pos_arg = np.mat(args[0][0])
00063                 rot_arg = np.mat(args[0][1])
00064                 if pos_arg.shape == (1, 3):
00065                     # matrix is row, convert to column
00066                     pos = pos_arg.T
00067                 elif pos_arg.shape == (3, 1):
00068                     pos = pos_arg
00069                 else:
00070                     return None, None, None, None
00071 
00072                 if rot_arg.shape == (3, 3):
00073                     # rotation matrix
00074                     homo_mat = np.mat(np.eye(4))
00075                     homo_mat[:3,:3] = rot_arg
00076                     quat = tf_trans.quaternion_from_matrix(homo_mat)
00077                     rot_euler = tf_trans.euler_from_matrix(homo_mat)
00078                 else:
00079                     if rot_arg.shape[1] == 1:
00080                         # make into row matrix
00081                         rot_arg = rot_arg.T
00082                     else:
00083                         rot_arg = rot_arg.tolist()[0]
00084                         if len(rot_arg) == 3:
00085                             # Euler angles rotation
00086                             homo_mat = np.mat(tf_trans.euler_matrix(*rot_arg))
00087                             quat = tf_trans.quaternion_from_euler(*rot_arg)
00088                             rot_euler = rot_arg
00089                         elif len(rot_arg) == 4:
00090                             # quaternion rotation
00091                             homo_mat = np.mat(tf_trans.quaternion_matrix(rot_arg))
00092                             quat = rot_arg
00093                             rot_euler = tf_trans.euler_from_quaternion(quat)
00094                         else:
00095                             return None, None, None, None
00096 
00097                 homo_mat[:3, 3] = pos
00098                 return None, homo_mat, np.array(quat), rot_euler
00099 
00100         elif len(args) == 2:
00101             header, homo_mat, rot_quat, rot_euler = PoseConverter._make_generic(
00102                                                                   ((args[0], args[1]),))
00103             if homo_mat is not None:
00104                 return header, homo_mat, rot_quat, rot_euler
00105 
00106         return None, None, None, None
00107 
00108     @staticmethod
00109     def _extract_pose_msg(pose):
00110         px = pose.position.x; py = pose.position.y; pz = pose.position.z
00111         ox = pose.orientation.x; oy = pose.orientation.y
00112         oz = pose.orientation.z; ow = pose.orientation.w
00113         quat = [ox, oy, oz, ow]
00114         rot_euler = tf_trans.euler_from_quaternion(quat)
00115         homo_mat = np.mat(tf_trans.quaternion_matrix(quat))
00116         homo_mat[:3,3] = np.mat([[px, py, pz]]).T
00117         return homo_mat, quat, rot_euler
00118 
00119     @staticmethod
00120     def _extract_tf_msg(tf_msg):
00121         px = tf_msg.translation.x; py = tf_msg.translation.y; pz = tf_msg.translation.z 
00122         ox = tf_msg.rotation.x; oy = tf_msg.rotation.y
00123         oz = tf_msg.rotation.z; ow = tf_msg.rotation.w
00124         quat = [ox, oy, oz, ow]
00125         rot_euler = tf_trans.euler_from_quaternion(quat)
00126         homo_mat = np.mat(tf_trans.quaternion_matrix(quat))
00127         homo_mat[:3,3] = np.mat([[px, py, pz]]).T
00128         return homo_mat, quat, rot_euler
00129 
00130     @staticmethod
00131     def _extract_twist_msg(twist_msg):
00132         pos = [twist_msg.linear.x, twist_msg.linear.y, twist_msg.linear.z]
00133         rot_euler = [twist_msg.angular.x, twist_msg.angular.y, twist_msg.angular.z]
00134         quat = tf_trans.quaternion_from_euler(*rot_euler, axes='sxyz')
00135         homo_mat = np.mat(tf_trans.euler_matrix(*rot_euler))
00136         homo_mat[:3,3] = np.mat([pos]).T
00137         return homo_mat, quat, rot_euler
00138 
00139     ##
00140     # @return geometry_msgs.Pose
00141     @staticmethod
00142     def to_pose_msg(*args):
00143         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00144         if homo_mat is None:
00145             rospy.logwarn("[pose_converter] Unknown pose type.")
00146             return None, None, None, None
00147         else:
00148             return Pose(Point(*homo_mat[:3,3].T.A[0]), Quaternion(*quat_rot))
00149 
00150     ##
00151     # @return geometry_msgs.PoseStamped
00152     @staticmethod
00153     def to_pose_stamped_msg(*args):
00154         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00155         if homo_mat is None:
00156             rospy.logwarn("[pose_converter] Unknown pose type.")
00157             return None, None, None, None
00158         ps = PoseStamped()
00159         if header is None:
00160             ps.header.stamp = rospy.Time.now()
00161         else:
00162             ps.header.seq = header[0]
00163             ps.header.stamp = header[1]
00164             ps.header.frame_id = header[2]
00165         ps.pose = Pose(Point(*homo_mat[:3,3].T.A[0]), Quaternion(*quat_rot))
00166         return ps
00167 
00168     ##
00169     # @return geometry_msgs.Point
00170     @staticmethod
00171     def to_point_msg(*args):
00172         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00173         if homo_mat is None:
00174             rospy.logwarn("[pose_converter] Unknown pose type.")
00175             return None, None, None, None
00176         return Point(*homo_mat[:3,3].T.A[0])
00177 
00178     ##
00179     # @return geometry_msgs.PointStamped
00180     @staticmethod
00181     def to_point_stamped_msg(*args):
00182         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00183         if homo_mat is None:
00184             rospy.logwarn("[pose_converter] Unknown pose type.")
00185             return None, None, None, None
00186         ps = PointStamped()
00187         if header is None:
00188             ps.header.stamp = rospy.Time.now()
00189         else:
00190             ps.header.seq = header[0]
00191             ps.header.stamp = header[1]
00192             ps.header.frame_id = header[2]
00193         ps.point = Point(*homo_mat[:3,3].T.A[0])
00194         return ps
00195 
00196     ##
00197     # @return geometry_msgs.Transform
00198     @staticmethod
00199     def to_tf_msg(*args):
00200         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00201         if homo_mat is None:
00202             rospy.logwarn("[pose_converter] Unknown pose type.")
00203             return None, None, None, None
00204         else:
00205             return Transform(Vector3(*homo_mat[:3,3].T.A[0]), Quaternion(*quat_rot))
00206 
00207     ##
00208     # @return geometry_msgs.TransformStamped
00209     @staticmethod
00210     def to_tf_stamped_msg(*args):
00211         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00212         if homo_mat is None:
00213             rospy.logwarn("[pose_converter] Unknown pose type.")
00214             return None, None, None, None
00215         tf_stamped = TransformStamped()
00216         if header is None:
00217             tf_stamped.header.stamp = rospy.Time.now()
00218         else:
00219             tf_stamped.header.seq = header[0]
00220             tf_stamped.header.stamp = header[1]
00221             tf_stamped.header.frame_id = header[2]
00222         tf_stamped.transform = Transform(Vector3(*homo_mat[:3,3].T.A[0]), Quaternion(*quat_rot))
00223         return tf_stamped
00224 
00225     ##
00226     # @return geometry_msgs.Twist
00227     @staticmethod
00228     def to_twist_msg(*args):
00229         _, homo_mat, _, euler_rot = PoseConverter._make_generic(args)
00230         if homo_mat is None:
00231             rospy.logwarn("[pose_converter] Unknown pose type.")
00232             return None, None, None, None
00233         else:
00234             return Twist(Vector3(*homo_mat[:3,3].T.A[0]), Vector3(*euler_rot))
00235 
00236     ##
00237     # @return geometry_msgs.TwistStamped
00238     @staticmethod
00239     def to_twist_stamped_msg(*args):
00240         header, homo_mat, _, euler_rot = PoseConverter._make_generic(args)
00241         if homo_mat is None:
00242             rospy.logwarn("[pose_converter] Unknown pose type.")
00243             return None, None, None, None
00244         twist_stamped = TwistStamped()
00245         header_msg = Header()
00246         if header is None:
00247             header_msg.stamp = rospy.Time.now()
00248         else:
00249             header_msg.seq = header[0]
00250             header_msg.stamp = header[1]
00251             header_msg.frame_id = header[2]
00252         return TwistStamped(header_msg, Twist(Vector3(*homo_mat[:3,3].T.A[0]), Vector3(*euler_rot)))
00253 
00254     ##
00255     # @return 4x4 numpy mat
00256     @staticmethod
00257     def to_homo_mat(*args):
00258         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00259         if homo_mat is None:
00260             rospy.logwarn("[pose_converter] Unknown pose type.")
00261             return None, None, None, None
00262         else:
00263             return homo_mat.copy()
00264 
00265     ##
00266     # @return (3x1 numpy mat, 3x3 numpy mat)
00267     @staticmethod
00268     def to_pos_rot(*args):
00269         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00270         if homo_mat is None:
00271             rospy.logwarn("[pose_converter] Unknown pose type.")
00272             return None, None, None, None
00273         else:
00274             return homo_mat[:3,3].copy(), homo_mat[:3,:3].copy()
00275 
00276     ##
00277     # @return (3 list, 4 list)
00278     @staticmethod
00279     def to_pos_quat(*args):
00280         header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
00281         if homo_mat is None:
00282             rospy.logwarn("[pose_converter] Unknown pose type.")
00283             return None, None, None, None
00284         else:
00285             return copy.copy(list(homo_mat[:3,3].T.A[0])), copy.copy(quat_rot)
00286 
00287     ##
00288     # @return (3 list, 3 list)
00289     @staticmethod
00290     def to_pos_euler(*args):
00291         header, homo_mat, quat_rot, euler_rot = PoseConverter._make_generic(args)
00292         if homo_mat is None:
00293             rospy.logwarn("[pose_converter] Unknown pose type.")
00294             return None, None, None, None
00295         else:
00296             return copy.copy(list(homo_mat[:3,3].T.A[0])), copy.copy(euler_rot)


hrl_generic_arms
Author(s): Kelsey Hawkins
autogenerated on Wed Nov 27 2013 11:53:37