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?
Post score: 5
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.