Go to the documentation of this file.00001 import numpy as np
00002 import tf.transformations as tr
00003
00004 def tf_as_matrix(tup):
00005 return np.matrix(tr.translation_matrix(tup[0])) * np.matrix(tr.quaternion_matrix(tup[1]))
00006
00007 def transform(to_frame, from_frame, tflistener, t=0):
00008 return tf_as_matrix(tflistener.lookupTransform(to_frame, from_frame, rospy.Time(t)))
00009
00010 def tf_as_matrix(tup):
00011 return np.matrix(tr.translation_matrix(tup[0])) * np.matrix(tr.quaternion_matrix(tup[1]))
00012
00013 def matrix_as_tf(mat):
00014 return (tr.translation_from_matrix(mat), tr.quaternion_from_matrix(mat))
00015
00016