Angle between two orientations

There are two orientations given, as intrinsic yaw-roll-pitch (rzyx).

How to calculate the angle between the "Z" axes of them?

Converting the orientations to quaternions or to anything else is allowed.

Originally posted by okalachev on ROS Answers with karma: 11 on 2018-08-29

Post score: 0

Comment by gvdhoorn on 2018-08-29:

Comment by okalachev on 2018-08-29:
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.

Comment by stevemacenski on 2018-08-29:
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.

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!

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-08-29

This answer was ACCEPTED on the original site

Post score: 1