# Multiply two tf transforms converted to 4x4 matrices in python? I want to do something along the lines of:

t = tf.getLatestCommonTime(l2, l4)
mat1 = transRotToMat4x4(tf.lookupTransform(l2, l1, t))
mat2 = transRotToMat4x4(tf.lookupTransform(l4, l3, t))
mat3 = mat1 * mat3

trans,rot = Mat4x4ToPosQuat(mat3)

br.sendTransform(
trans,
rot,
t,
"target",
"source");


Does transformations.py have something like transRotToMat4x4 and the complementary function for going the other direction?

edit retag close merge delete

Sort by » oldest newest most voted The transformations.py included in tf provided the functions for the solution, they create 4x4 numpy nd.arrays which are matrix multiplied with dot():

(trans1, rot1) = tf.lookupTransform(l2, l1, t)
trans1_mat = tf.transformations.translation_matrix(trans1)
rot1_mat   = tf.transformations.quaternion_matrix(rot1)
mat1 = numpy.dot(trans1_mat, rot1_mat)

(trans2, rot2) = tf.lookupTransform(l4, l3, t)
trans2_mat = tf.transformations.translation_matrix(trans2)
rot2_mat    = tf.transformations.quaternion_matrix(rot2)
mat2 = numpy.dot(trans2_mat, rot2_mat)

mat3 = numpy.dot(mat1, mat2)
trans3 = tf.transformations.translation_from_matrix(mat3)
rot3 = tf.transformations.quaternion_from_matrix(mat3)

br.sendTransform(
trans3,
rot3,
t,
"target",
"source");

more