Go to the documentation of this file.00001 from __future__ import print_function
00002
00003 import numbers
00004
00005 from tf.transformations import *
00006 from geometry_msgs.msg import Pose
00007
00008
00009
00010 def rounded(val):
00011 if isinstance(val, str):
00012 return rounded(float(val))
00013 elif isinstance(val, numbers.Number):
00014 return int(round(val,6) * 1e5) / 1.0e5
00015 else:
00016 return numpy.array([rounded(v) for v in val])
00017
00018
00019 def homogeneous2translation_quaternion(homogeneous):
00020 """
00021 Translation: [x, y, z]
00022 Quaternion: [x, y, z, w]
00023 """
00024 translation = translation_from_matrix(homogeneous)
00025 quaternion = quaternion_from_matrix(homogeneous)
00026 return translation, quaternion
00027
00028
00029 def homogeneous2translation_rpy(homogeneous):
00030 """
00031 Translation: [x, y, z]
00032 RPY: [sx, sy, sz]
00033 """
00034 translation = translation_from_matrix(homogeneous)
00035 rpy = euler_from_matrix(homogeneous)
00036 return translation, rpy
00037
00038
00039 def homogeneous2pose_msg(homogeneous):
00040 pose = Pose()
00041 translation, quaternion = homogeneous2translation_quaternion(homogeneous)
00042 pose.position.x = translation[0]
00043 pose.position.y = translation[1]
00044 pose.position.z = translation[2]
00045 pose.orientation.x = quaternion[0]
00046 pose.orientation.y = quaternion[1]
00047 pose.orientation.z = quaternion[2]
00048 pose.orientation.w = quaternion[3]
00049 return pose
00050
00051
00052 def pose_msg2homogeneous(pose):
00053 trans = translation_matrix((pose.position.x, pose.position.y, pose.position.z))
00054 rot = quaternion_matrix((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w))
00055 return concatenate_matrices(trans, rot)
00056
00057
00058 def array2string(array):
00059 return numpy.array_str(array).strip('[]. ').replace('. ', ' ')
00060
00061
00062 def homogeneous2tq_string(homogeneous):
00063 return 't=%s q=%s' % homogeneous2translation_quaternion(homogeneous)
00064
00065
00066 def homogeneous2tq_string_rounded(homogeneous):
00067 return 't=%s q=%s' % tuple(rounded(o) for o in homogeneous2translation_quaternion(homogeneous))
00068
00069
00070 def string2float_list(s):
00071 return [float(i) for i in s.split()]
00072
00073
00074 def pose_string2homogeneous(pose):
00075 pose_float = string2float_list(pose)
00076 translate = pose_float[:3]
00077 angles = pose_float[3:]
00078 homogeneous = compose_matrix(None, None, angles, translate)
00079
00080 return homogeneous
00081
00082
00083 def rotation_only(homogeneous):
00084 euler = euler_from_matrix(homogeneous)
00085 return euler_matrix(euler[0], euler[1], euler[2])