ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Multiply two tf transforms converted to 4x4 matrices in python?

asked 2014-02-25 12:17:39 -0500

lucasw gravatar image

updated 2020-11-21 11:27:22 -0500

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 = tf.TransformBroadcaster()
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 flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2014-02-26 04:12:02 -0500

lucasw gravatar image

updated 2014-02-26 04:12:44 -0500

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 = tf.TransformBroadcaster()
br.sendTransform(
  trans3,
  rot3,
  t,
  "target",
  "source");
edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2014-02-25 12:17:39 -0500

Seen: 5,528 times

Last updated: Feb 26 '14