| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
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?
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.
Just to clarify my understanding, does this transform pose2 to the pose1 reference frame? The new pose data is written to pose1? Thanks!
@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?
Asked: 2013-04-14 22:14:41 -0500
Seen: 13,841 times
Last updated: Apr 14 '13
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
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.
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.
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.