Calculate difference between two poses

Question

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?

---

Originally posted by Robbiepr1 on ROS Answers with karma: 143 on 2013-04-14

Post score: 5

---

Original comments

Comment by davinci on 2013-04-14:
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.

Comment by Robbiepr1 on 2013-04-14:
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.

Comment by Martin Günther on 2013-04-14:
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.

Answer 1

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.

---

Originally posted by dornhege with karma: 31395 on 2013-04-14

This answer was ACCEPTED on the original site

Post score: 18

---

Original comments

Comment by Robbiepr1 on 2013-04-15:
Thanks, that did the trick

Comment by Mehdi. on 2015-08-11:
How does this even work when you are using Pose from geometry_msgs?

Comment by 2ROS0 on 2015-12-20:
@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.

Comment by i_robot_flight on 2017-11-10:
Just to clarify my understanding, does this transform pose2 to the pose1 reference frame? The new pose data is written to pose1? Thanks!

Comment by sanazir on 2017-11-26:
any idea how to do this in python?

Comment by simff on 2018-02-21:
@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?