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

# Calculate difference between two poses

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 close merge delete

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.

( 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.

( 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.

( 2013-04-14 23:03:19 -0600 )edit

Sort by » oldest newest most voted

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.

more

Thanks, that did the trick

( 2013-04-15 01:21:54 -0600 )edit
2

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

( 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.

( 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!

( 2017-11-10 11:39:26 -0600 )edit
3

any idea how to do this in python?

( 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?

( 2018-02-21 04:17:56 -0600 )edit