ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

Chain transformations manually

asked 2021-10-12 03:46:11 -0600

JayDe gravatar image

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!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-10-12 07:36:10 -0600

Mike Scheutzow gravatar image

updated 2021-10-12 09:14:58 -0600

Your code looks correct to me. When I copy/paste your code, I get the expected answer:

>>> print(world_to_ir_matrix)
[[ 0.99879572 -0.01262108  0.04741123 -0.83998475]
 [-0.0415123  -0.73246292  0.67954014 -1.05468425]
 [ 0.02615044 -0.68068993 -0.73210476  2.80150618]
 [ 0.          0.          0.          1.        ]]
>>> print(tf.quaternion_from_matrix(world_to_ir_matrix))
[ 0.9305057  -0.01454408  0.0197639  -0.36545452]

I personally prefer to use c = tf.transformations.concatenate_matricies(a, b) rather than c = a.dot(b) because it's easier for readers to understand.

Additional info requested:

>>> print(tf.quaternion_matrix([-0.252, 0.245, 0.645, 0.679] ))
[[ 0.04846616 -0.99879572  0.00762546  0.        ]
 [ 0.75198257  0.0415123   0.65787456  0.        ]
 [-0.65739885 -0.02615044  0.75308891  0.        ]
 [ 0.          0.          0.          1.        ]]

>>> print(world_to_link_matrix)
[[ 0.04846616 -0.99879572  0.00762546 -0.84      ]
 [ 0.75198257  0.0415123   0.65787456 -1.056     ]
 [-0.65739885 -0.02615044  0.75308891  2.8       ]
 [ 0.          0.          0.          1.        ]]

>>> print(link_to_ir_matrix)
[[ -3.33066907e-16  -1.03926273e-01   9.94585004e-01   0.00000000e+00]
 [ -1.00000000e+00  -3.33066907e-16   0.00000000e+00   0.00000000e+00]
 [  0.00000000e+00  -9.94585004e-01  -1.03926273e-01   2.00000000e-03]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e+00]]
edit flag offensive delete link more

Comments

Hi Mike! My world_to_ir_matrix looks different. Could you print out world_to_link_matrix and link_to_ir_matrix as well? Thanks!

JayDe gravatar image JayDe  ( 2021-10-12 08:47:54 -0600 )edit

transformations is using the numpy library. If you have reconfigured how numpy behaves, you can break these functions.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-10-12 09:16:48 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-10-12 03:46:11 -0600

Seen: 68 times

Last updated: Oct 12 '21