Ask Your Question

Revision history [back]

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)
(trans2, rot2) = tf.lookupTransform(l4, l3, t)
trans1_mat = tf.transformations.translation_matrix(godseye_trans)
rot1_mat   = tf.transformations.quaternion_matrix(godseye_quat)
mat1 = numpy.dot(trans1_mat, rot1_mat)

trans2_mat = tf.transformations.translation_matrix(odom_trans)
rot2_mat    = tf.transformations.quaternion_matrix(odom_quat)
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");

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)
trans1_mat = tf.transformations.translation_matrix(godseye_trans)
rot1_mat   = tf.transformations.quaternion_matrix(godseye_quat)
mat1 = numpy.dot(trans1_mat, rot1_mat)

trans2_mat = tf.transformations.translation_matrix(odom_trans)
tf.transformations.translation_matrix(trans2)
rot2_mat    = tf.transformations.quaternion_matrix(odom_quat)
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");