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

Calculate difference between two poses

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

Robbiepr1 gravatar image

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

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 image davinci  ( 2013-04-14 22:40:49 -0600 )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 image Robbiepr1  ( 2013-04-14 22:50:41 -0600 )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 image Martin Günther  ( 2013-04-14 23:03:19 -0600 )edit

1 Answer

Sort by » oldest newest most voted
18

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

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 image Robbiepr1  ( 2013-04-15 01:21:54 -0600 )edit
2

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

Mehdi. gravatar image Mehdi.  ( 2015-08-11 09:10:01 -0600 )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 image 2ROS0  ( 2015-12-19 23:36:52 -0600 )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 image i_robot_flight  ( 2017-11-10 11:39:26 -0600 )edit
3

any idea how to do this in python?

sanazir gravatar image sanazir  ( 2017-11-26 21:09:49 -0600 )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 image simff  ( 2018-02-21 04:17:56 -0600 )edit

Question Tools

Stats

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

Seen: 14,398 times

Last updated: Apr 14 '13