Chain transformations manually
Hi there! I am using ROS Noetic on an Ubuntu 20.04 machine. What I am trying to do is to calculate a chained transformation myself. To do so I printed the two individual transformations using tf_echo like so:
rosrun tf tf_echo world link
At time 0.000
- Translation: [-0.840, -1.056, 2.800]
- Rotation: in Quaternion [-0.252, 0.245, 0.645, 0.679]
and the second one:
rosrun tf tf_echo link ir
At time 0.000
- Translation: [0.000, 0.000, 0.002]
- Rotation: in Quaternion [0.525, -0.525, 0.473, -0.473]
I know want to compute
world -> ir
"manually". I know what the result should be by asking
rosrun tf tf_echo world ir
At time 0.000
- Translation: [-0.840, -1.055, 2.802]
- Rotation: in Quaternion [0.931, -0.015, 0.020, -0.365]
which I can't reproduce. I am using the python "transformations" package to attempt it. My computation is as follows:
world_to_link_matrix = tf.translation_matrix([-0.840, -1.056, 2.800]).dot(tf.quaternion_matrix([-0.252, 0.245, 0.645, 0.679] ))
link_to_ir_matrix = tf.translation_matrix([0.000, 0.000, 0.002]).dot(tf.quaternion_matrix([0.525, -0.525, 0.473, -0.473]))
world_to_ir_matrix = world_to_link_matrix.dot(link_to_ir_matrix)
result_trans = tf.translation_from_matrix(world_to_ir_matrix)
result_quat = tf.quaternion_from_matrix(world_to_ir_matrix) # gives me [0.01279722, -0.36484653, -0.02118857, 0.93073857]
What is going on / what am I doing wrong? Thank you!