# 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