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

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

edit retag close merge delete

( 2018-08-29 12:14:50 -0600 )edit

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.

( 2018-08-29 12:34:59 -0600 )edit

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.

( 2018-08-29 13:16:57 -0600 )edit

Sort by ยป oldest newest most voted

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!

more

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

( 2018-08-30 13:32:52 -0600 )edit

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.

( 2018-08-30 15:35:53 -0600 )edit