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

what is the right way to inverse a transform in python

asked 2016-03-17 05:53:53 -0600

lgeorge gravatar image

I am wondering if you know a better way (less boiler code) that will allow me to inverse a transform msg, in Python. For now I use the following code:

from geometry_msgs.msg import PoseStamped, TransformStamped
from tf import TransformerROS

res = TransformStamped()  # creating new transform msg
res.header.stamp = input_transform.header.stamp  # same timestamp
res.header.frame_id = input_transform.child_frame_id  # inverting frame_id and child_frame_id manualy
res.child_frame_id = input_transform.header.frame_id
transformer = TransformerROS()  # using a transformer in order to compute inverse transform
transformer.setTransform(input_transform)  # using the input_transform
translation, rotation = transformer.lookupTransform(res.header.frame_id, res.child_frame_id, rospy.Time(0))  # using lookup in order to get the inverse of input transform (here I use input_transform.child_frame_id to input_transform_frame_id
for attr_name, val in zip(['x', 'y', 'z'], translation):
    res.transform.translation.__setattr__(attr_name, val)
for attr_name, val in zip(['x', 'y', 'z', 'w'], rotation):
    res.transform.rotation.__setattr__(attr_name, val)

what do you think ?

edit retag flag offensive close merge delete

Comments

1

setattr(x, y) should always be preferred over x.__setattr__(y)

eric-wieser gravatar image eric-wieser  ( 2016-03-17 10:16:57 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
8

answered 2016-03-17 07:36:54 -0600

Airuno2L gravatar image

updated 2016-03-17 07:37:42 -0600

There are a million ways to do this and none of them are really right or wrong. But personally, I like to use numpy (actually the tf.transformations library which is a nice wrapper for numpy) for everything inside my code.

I only use the transform messages to receive and send transforms to other processes. So as soon as any of my software takes in a transform, pose, point, or whatever, and I know I need to manipulate it, the first thing I do is convert it to a numpy type. I think always working with the same types and library makes things cleaner and easier in the long run.

So if you're grabbing a transform and want to invert it, that would look like this:

from tf import transformations as t

(trans, rot) = transformer.lookupTransform(frame1, frame2, rospy.Time(0))
transform = t.concatenate_matrices(t.translation_matrix(trans), t.quaternion_matrix(rot))
inversed_transform = t.inverse_matrix(transform)

Is that what you're looking for?

edit flag offensive delete link more

Comments

Can you add making the inversed_transform back into a translation vector and quaternion?

lucasw gravatar image lucasw  ( 2017-02-22 16:06:01 -0600 )edit
3

I think it's something like this:

translation = t.translation_from_matrix(inversed_transform)
quaternion = t.quaternion_from_matrix(inversed_transform)

The transformations.py is documented well for future reference.

Airuno2L gravatar image Airuno2L  ( 2017-02-25 14:52:38 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2016-03-17 05:53:53 -0600

Seen: 9,222 times

Last updated: Mar 17 '16