| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
I'd suggest converting both of the orientations into transformation objects, then create a point object 1m along the Z axis. Then use the two transformations to transform the point from the frame of one transform into the other. This will give you two unit vectors one if which is (0 0 1)^ the other which is something else, you can then use the dot product to find the angle between the two.
In a little bit more detail you'd create the two transform objects like this.
tf2::Quaternion q1;
q1.setRPY(roll, pitch, yaw); // node the order
tf2::Transform t1(q1);
Do the same with q2 and t2 for the other orientation then create the Z axis unit point.
tf2::Vector3 zPoint(0,0,1);
Transform it from one frame to the other.
tf2::Vector3 otherZPoint = t1.inverseTimes(t2) * zPoint;
Now you can calculate the angle between the two unit vectors zPoint and otherZPoint as so.
float angle = acos(zPoint.dot(otherZPoint) );
It's a bit long winded but I hope this solves the problem you're asking about!
Yes, I think this is a working variant!
Isn't there an easier solution, without converting to transforms? I was given this formula: math.atan(math.hypot(math.tan(pitch), math.tan(roll))), it almost works, but doesn't give values more than 90 degrees (when Z axes are "upside down" to each other).
I'm sure it could be simplified, the question is do you need to? Is performance critical for this calculation? If not and you've got a working solution then I'd say the job's done.
Asked: 2018-08-29 12:13:46 -0500
Seen: 2,456 times
Last updated: Aug 29 '18
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.
Does #q10124 not answer this? See also tf2/Tutorials/Quaternions.
Looks like it's the finding of the full rotation between orientations. What I need, it the single angle between the Z axes of orientations.
I'd look into the axis angle representation. Alternatively, what you're describing could be interpreted as angular _distance_ which you can formulate the same way you might an L2 distance.