Ask Your Question
5

Calculate difference between two poses

asked 2013-04-14 22:14:41 -0500

Robbiepr1 gravatar image

updated 2014-01-28 17:16:10 -0500

ngrennan gravatar image

I running in to a problem calculating the difference between two tf poses. Currently I calculate the difference by subtracting the X, Y and Z (orientation) of pose 1 from pose 2. This works fine. Only when I try to do the same for the rotation (XYZW) the result is (sometimes) NaN. Am I doing something wrong or is there a better way to calculate the difference between two poses?

edit retag flag offensive close merge delete

Comments

Are you using the c++ tf method? Sometimes a transform is not available yet, you can use waitForTransform(). Try also to print out the numbers you are subtracting.

davinci gravatar imagedavinci ( 2013-04-14 22:40:49 -0500 )edit

Thanks for your reply. The situation is that I already have two poses (both of type tf::Stamped<tf::Pose> and can even have the same frame_id) and I want to get the difference between those two poses. I already printed the numbers I'm subtracting and they seem valid numbers.

Robbiepr1 gravatar imageRobbiepr1 ( 2013-04-14 22:50:41 -0500 )edit
3

First of all, you cannot simply subtract two rotations, since they (and the result) must be valid quaternions; for example, sqrt(x^2 + y^2 +z^2 +w^2) must be = 1. Still, you should only get NaN after subtraction if one (or both) of the input values are already NaN.

Martin Günther gravatar imageMartin Günther ( 2013-04-14 23:03:19 -0500 )edit

1 Answer

Sort by » oldest newest most voted
14

answered 2013-04-14 23:48:32 -0500

dornhege gravatar image

TF provides a correct way for this:

tfpose1.inverseTimes(tfpose2)

This is the transformation from pose1 to pose2 or "pose2 - pose1" (with the '-' not being a real minus, but the correct operator). The resulting pose will have a transformation and orientation that is the difference of the two poses.

edit flag offensive delete link more

Comments

Thanks, that did the trick

Robbiepr1 gravatar imageRobbiepr1 ( 2013-04-15 01:21:54 -0500 )edit
2

How does this even work when you are using Pose from geometry_msgs?

Mehdi. gravatar imageMehdi. ( 2015-08-11 09:10:01 -0500 )edit

@Mehdi.

You can do the same by converting geometry_msgs Pose to tf Transform using the file tf/transform_datatypes.h which defines these data type transformations.

2ROS0 gravatar image2ROS0 ( 2015-12-19 23:36:52 -0500 )edit

Just to clarify my understanding, does this transform pose2 to the pose1 reference frame? The new pose data is written to pose1? Thanks!

i_robot_flight gravatar imagei_robot_flight ( 2017-11-10 11:39:26 -0500 )edit
1

any idea how to do this in python?

sanazir gravatar imagesanazir ( 2017-11-26 21:09:49 -0500 )edit

@Robbiepr1 How did you save the result of your subtraction? For me using subtract = tfpose1.inverseTimes(tfpose2) did not work! I would also need that method too. Anyone knows?

simff gravatar imagesimff ( 2018-02-21 04:17:56 -0500 )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

Stats

Asked: 2013-04-14 22:14:41 -0500

Seen: 6,293 times

Last updated: Apr 14 '13